From 1dc9785083e1ed7a7db27466fbe9f61f6bb277f7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 Jan 2014 16:52:35 +0100 Subject: OFFBOARD switch added, WIP --- src/modules/commander/commander.cpp | 11 +++++++++ src/modules/sensors/sensor_params.c | 3 +-- src/modules/sensors/sensors.cpp | 29 ++++++++++------------- src/modules/uORB/topics/manual_control_setpoint.h | 4 +--- src/modules/uORB/topics/vehicle_status.h | 8 +++++++ 5 files changed, 34 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 47053838c..e2b6ad1ca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1410,6 +1410,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else { current_status->mission_switch = MISSION_SWITCH_MISSION; } + + /* offboard switch */ + if (!isfinite(sp_man->offboard_switch)) { + current_status->offboard_switch = OFFBOARD_SWITCH_NONE; + + } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { + current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + + } else { + current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; + } } transition_result_t diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bbc84ef93..eb771382d 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -390,8 +390,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); - -//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5e8c5edbc..f87f2f282 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -246,8 +246,7 @@ private: int rc_map_return_sw; int rc_map_assisted_sw; int rc_map_mission_sw; - -// int rc_map_offboard_ctrl_mode_sw; + int rc_map_offboard_sw; int rc_map_flaps; @@ -296,8 +295,7 @@ private: param_t rc_map_return_sw; param_t rc_map_assisted_sw; param_t rc_map_mission_sw; - -// param_t rc_map_offboard_ctrl_mode_sw; + param_t rc_map_offboard_sw; param_t rc_map_flaps; @@ -515,8 +513,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); - -// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + _parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -669,14 +666,14 @@ Sensors::parameters_update() warnx("Failed getting mission sw chan index"); } + if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) { + warnx("Failed getting offboard sw chan index"); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } -// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { -// warnx("Failed getting offboard control mode sw chan index"); -// } - param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); @@ -700,11 +697,10 @@ Sensors::parameters_update() _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; -// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; - _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; @@ -1289,7 +1285,7 @@ Sensors::rc_poll() manual_control.return_switch = NAN; manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; -// manual_control.auto_offboard_input_switch = NAN; + manual_control.offboard_switch = NAN; manual_control.flaps = NAN; manual_control.aux1 = NAN; @@ -1432,9 +1428,10 @@ Sensors::rc_poll() manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); } -// if (_rc.function[OFFBOARD_MODE] >= 0) { -// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); -// } + /* offboard switch input */ + if (_rc.function[OFFBOARD_MODE] >= 0) { + manual_control.offboard_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + } /* aux functions, only assign if valid mapping is present */ if (_rc.function[AUX_1] >= 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e98..ab0c7a411 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -60,15 +60,13 @@ struct manual_control_setpoint_s { float return_switch; /**< land 2 position switch (mandatory): land, no effect */ float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + float offboard_switch; /**< offboard switch (optional): offboard, onboard */ /** * Any of the channels below may not be available and be set to NaN * to indicate that it does not contain valid data. */ - // XXX needed or parameter? - //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - float flaps; /**< flap position */ float aux1; /**< default function: camera yaw / azimuth */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f5..d74b696bb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -64,6 +64,7 @@ typedef enum { MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, + MAIN_STATE_OFFBOARD, } main_state_t; typedef enum { @@ -109,6 +110,12 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +typedef enum { + OFFBOARD_SWITCH_NONE = 0, + OFFBOARD_SWITCH_OFFBOARD, + OFFBOARD_SWITCH_ONBOARD +} offboard_switch_pos_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -184,6 +191,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; + offboard_switch_pos_t offboard_switch; bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ -- cgit v1.2.3 From 37e28e7fa82a965967abba6650ba24a31aba10d3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 Jan 2014 17:04:07 +0100 Subject: commander: OFFBOARD mode added, WIP --- src/modules/commander/commander.cpp | 80 ++++++++++++++------------ src/modules/commander/state_machine_helper.cpp | 6 ++ 2 files changed, 49 insertions(+), 37 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e2b6ad1ca..dfcb1cc9f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1429,56 +1429,62 @@ check_main_state_machine(struct vehicle_status_s *current_status) /* evaluate the main state machine */ transition_result_t res = TRANSITION_DENIED; - switch (current_status->mode_switch) { - case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + if (current_status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { + /* offboard switch overrides main switch */ + res = main_state_transition(current_status, MAIN_STATE_OFFBOARD); - case MODE_SWITCH_ASSISTED: - if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY); + } else { + switch (current_status->mode_switch) { + case MODE_SWITCH_MANUAL: + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + case MODE_SWITCH_ASSISTED: + if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(current_status, MAIN_STATE_EASY); - // else fallback to SEATBELT - print_reject_mode("EASY"); - } + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + // else fallback to SEATBELT + print_reject_mode("EASY"); + } - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this mode + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); - if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages - print_reject_mode("SEATBELT"); + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this mode - // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + print_reject_mode("SEATBELT"); + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO); + case MODE_SWITCH_AUTO: + res = main_state_transition(current_status, MAIN_STATE_AUTO); - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - // else fallback to SEATBELT (EASY likely will not work too) - print_reject_mode("AUTO"); - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + // else fallback to SEATBELT (EASY likely will not work too) + print_reject_mode("AUTO"); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - default: - break; + default: + break; + } } return res; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 731e0e3ff..e5599e1c3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -262,6 +262,12 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m ret = TRANSITION_CHANGED; } + break; + + case MAIN_STATE_OFFBOARD: + + ret = TRANSITION_CHANGED; + break; } -- cgit v1.2.3 From f069fe9f61d24b5a6cc66d60afd3f8b5e568824a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 Jan 2014 19:25:14 +0100 Subject: OFFBOARD mode support in commander, mavlink, mc_att_control, WIP --- src/modules/commander/commander.cpp | 66 +++++++++------ src/modules/commander/px4_custom_mode.h | 1 + src/modules/mavlink/mavlink.c | 2 + src/modules/mc_att_control/mc_att_control_main.cpp | 99 ++++++++++++---------- src/modules/navigator/navigator_main.cpp | 16 +++- 5 files changed, 110 insertions(+), 74 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dfcb1cc9f..7b69f2b76 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -411,42 +411,52 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set main state */ transition_result_t main_res = TRANSITION_DENIED; - if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { - /* use autopilot-specific mode */ - if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { - /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); - - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); - - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); - } + if (status->rc_signal_lost) { + /* allow mode switching by command only if no RC signal */ + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else { - /* use base mode */ - if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { - if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { - /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { + /* OFFBOARD */ + main_res = main_state_transition(status, MAIN_STATE_OFFBOARD); + } + + } else { + /* use base mode */ + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } } } + } else { + mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd"); } + if (main_res == TRANSITION_CHANGED) ret = true; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9..4843e72c3 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -13,6 +13,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_SEATBELT, PX4_CUSTOM_MAIN_MODE_EASY, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_MAIN_MODE_OFFBOARD, }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4f8091716..70229558c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -231,6 +231,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (control_mode.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } + } else if (v_status.main_state == MAIN_STATE_OFFBOARD) { + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 245ac024b..c4c6c0138 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -480,64 +480,73 @@ MulticopterAttitudeControl::task_main() float yaw_sp_move_rate = 0.0f; bool publish_att_sp = false; - /* define which input is the dominating control input */ - if (_control_mode.flag_control_manual_enabled) { - /* manual input */ - if (!_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - _att_sp.thrust = _manual.throttle; - } + if (_control_mode.flag_control_offboard_enabled) { + /* offboard control */ + // TODO set _att_sp or _rates_sp here depending on offboard control mode + // TODO _control_mode.flag_control_XXX flags are all false, set it according to ofboard control mode + // but it's not very beautiful, need to think how to do it better - if (!_arming.armed) { - /* reset yaw setpoint when disarmed */ - reset_yaw_sp = true; - } + } else { + /* onboard control */ + /* define which input is the dominating control input */ + if (_control_mode.flag_control_manual_enabled) { + /* manual input */ + if (!_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + _att_sp.thrust = _manual.throttle; + } - if (_control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ + if (!_arming.armed) { + /* reset yaw setpoint when disarmed */ + reset_yaw_sp = true; + } - if (_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual.yaw; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); + if (_control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ + + if (_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual.yaw; + _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); + _att_sp.R_valid = false; + publish_att_sp = true; + } + } + + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp.yaw_body = _att.yaw; _att_sp.R_valid = false; publish_att_sp = true; } - } - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; - _att_sp.R_valid = false; - publish_att_sp = true; - } + if (!_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _att_sp.roll_body = _manual.roll; + _att_sp.pitch_body = _manual.pitch; + _att_sp.R_valid = false; + publish_att_sp = true; + } - if (!_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _att_sp.roll_body = _manual.roll; - _att_sp.pitch_body = _manual.pitch; - _att_sp.R_valid = false; - publish_att_sp = true; + } else { + /* manual rate inputs (ACRO) */ + // TODO + /* reset yaw setpoint after ACRO */ + reset_yaw_sp = true; } } else { - /* manual rate inputs (ACRO) */ - // TODO - /* reset yaw setpoint after ACRO */ + /* reset yaw setpoint after non-manual control */ reset_yaw_sp = true; } - - } else { - /* reset yaw setpoint after non-manual control */ - reset_yaw_sp = true; } if (_att_sp.R_valid) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ca5735509..7285e243e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1463,11 +1463,11 @@ Navigator::publish_control_mode() _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; - _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_flighttermination_enabled = false; switch (_vstatus.main_state) { case MAIN_STATE_MANUAL: + _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = true; _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; @@ -1478,6 +1478,7 @@ Navigator::publish_control_mode() break; case MAIN_STATE_SEATBELT: + _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = true; _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1488,6 +1489,7 @@ Navigator::publish_control_mode() break; case MAIN_STATE_EASY: + _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = true; _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1498,6 +1500,7 @@ Navigator::publish_control_mode() break; case MAIN_STATE_AUTO: + _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = false; if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ @@ -1517,6 +1520,17 @@ Navigator::publish_control_mode() } break; + case MAIN_STATE_OFFBOARD: + _control_mode.flag_control_offboard_enabled = true; + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + default: break; } -- cgit v1.2.3 From 6bb85a323ce7f152aead0c9133d86e5e900bd7ba Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Thu, 23 Jan 2014 11:01:22 +0000 Subject: added offboard data handling in mc_att_control_main.cpp --- src/modules/mc_att_control/mc_att_control_main.cpp | 67 +++++++++++++++++++++- 1 file changed, 64 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c4c6c0138..129e5df8f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -118,6 +119,7 @@ private: int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ + int _offboard_sub; /**< notification of offboard control updates */ int _arming_sub; /**< arming status of outputs */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ @@ -127,6 +129,7 @@ private: struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct offboard_control_setpoint_s _offboard; /**< offboard data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_armed_s _arming; /**< actuator arming status */ @@ -173,6 +176,11 @@ private: * Check for changes in manual inputs. */ void vehicle_manual_poll(); + + /** + * Check for changes in offboard inputs. + */ + void vehicle_offboard_poll(); /** * Check for set triplet updates. @@ -339,6 +347,23 @@ MulticopterAttitudeControl::vehicle_manual_poll() } } +void +MulticopterAttitudeControl::vehicle_offboard_poll() +{ + bool offboard_updated; + + /* get offboard inputs */ + orb_check(_offboard_sub, &offboard_updated); + + if (offboard_updated) { + + orb_copy(ORB_ID(offboard_control_setpoint), _offboard_sub, &_offboard); + } + +} + + + void MulticopterAttitudeControl::vehicle_setpoint_poll() { @@ -363,6 +388,8 @@ MulticopterAttitudeControl::arming_status_poll() } } + + void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -476,15 +503,49 @@ MulticopterAttitudeControl::task_main() vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); + vehicle_offboard_poll(); float yaw_sp_move_rate = 0.0f; bool publish_att_sp = false; if (_control_mode.flag_control_offboard_enabled) { - /* offboard control */ - // TODO set _att_sp or _rates_sp here depending on offboard control mode + /* offboard control */ + // TODO set _att_sp or _rates_sp here depending on offboard control mode // TODO _control_mode.flag_control_XXX flags are all false, set it according to ofboard control mode // but it's not very beautiful, need to think how to do it better + + // If control mode is rates set _rates_sp + if(_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { + _rates_sp.roll = _offboard.p1; + _rates_sp.pitch = _offboard.p2; + _rates_sp.yaw = _offboard.p3; + _rates_sp.roll = _offboard.p4; + + // If control mode is atitude set _att_sp + } else if (_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + _control_mode.flag_control_attitude_enabled = true; + + + /* pass throttle directly if not in altitude control mode */ + if (!_control_mode.flag_control_climb_rate_enabled) { + _att_sp.thrust = _offboard.p4; + } + + if (!_arming.armed) { + /* reset yaw setpoint when disarmed */ + _att_sp.yaw_body = _att.yaw; + } + else{ + _att_sp.yaw_body = _offboard.p3; + } + + _att_sp.roll_body = _offboard.p1; + _att_sp.pitch_body = _offboard.p2; + + _att_sp.R_valid = false; + publish_att_sp = true; + } + } else { /* onboard control */ @@ -565,7 +626,7 @@ MulticopterAttitudeControl::task_main() if (publish_att_sp) { /* publish the attitude setpoint */ _att_sp.timestamp = hrt_absolute_time(); - +printf("att_sp RPYT: %.2f %.2f %.2f %.2f", _att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body, _att_sp.thrust) if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); -- cgit v1.2.3 From d4ae1c01adea93f23b71e742346b2d892204716c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 12:24:04 +0100 Subject: Revert "added offboard data handling in mc_att_control_main.cpp" This reverts commit 6bb85a323ce7f152aead0c9133d86e5e900bd7ba. --- src/modules/mc_att_control/mc_att_control_main.cpp | 67 +--------------------- 1 file changed, 3 insertions(+), 64 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 129e5df8f..c4c6c0138 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -63,7 +63,6 @@ #include #include #include -#include #include #include #include @@ -119,7 +118,6 @@ private: int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ - int _offboard_sub; /**< notification of offboard control updates */ int _arming_sub; /**< arming status of outputs */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ @@ -129,7 +127,6 @@ private: struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct offboard_control_setpoint_s _offboard; /**< offboard data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_armed_s _arming; /**< actuator arming status */ @@ -176,11 +173,6 @@ private: * Check for changes in manual inputs. */ void vehicle_manual_poll(); - - /** - * Check for changes in offboard inputs. - */ - void vehicle_offboard_poll(); /** * Check for set triplet updates. @@ -347,23 +339,6 @@ MulticopterAttitudeControl::vehicle_manual_poll() } } -void -MulticopterAttitudeControl::vehicle_offboard_poll() -{ - bool offboard_updated; - - /* get offboard inputs */ - orb_check(_offboard_sub, &offboard_updated); - - if (offboard_updated) { - - orb_copy(ORB_ID(offboard_control_setpoint), _offboard_sub, &_offboard); - } - -} - - - void MulticopterAttitudeControl::vehicle_setpoint_poll() { @@ -388,8 +363,6 @@ MulticopterAttitudeControl::arming_status_poll() } } - - void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -503,49 +476,15 @@ MulticopterAttitudeControl::task_main() vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); - vehicle_offboard_poll(); float yaw_sp_move_rate = 0.0f; bool publish_att_sp = false; if (_control_mode.flag_control_offboard_enabled) { - /* offboard control */ - // TODO set _att_sp or _rates_sp here depending on offboard control mode + /* offboard control */ + // TODO set _att_sp or _rates_sp here depending on offboard control mode // TODO _control_mode.flag_control_XXX flags are all false, set it according to ofboard control mode // but it's not very beautiful, need to think how to do it better - - // If control mode is rates set _rates_sp - if(_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { - _rates_sp.roll = _offboard.p1; - _rates_sp.pitch = _offboard.p2; - _rates_sp.yaw = _offboard.p3; - _rates_sp.roll = _offboard.p4; - - // If control mode is atitude set _att_sp - } else if (_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - _control_mode.flag_control_attitude_enabled = true; - - - /* pass throttle directly if not in altitude control mode */ - if (!_control_mode.flag_control_climb_rate_enabled) { - _att_sp.thrust = _offboard.p4; - } - - if (!_arming.armed) { - /* reset yaw setpoint when disarmed */ - _att_sp.yaw_body = _att.yaw; - } - else{ - _att_sp.yaw_body = _offboard.p3; - } - - _att_sp.roll_body = _offboard.p1; - _att_sp.pitch_body = _offboard.p2; - - _att_sp.R_valid = false; - publish_att_sp = true; - } - } else { /* onboard control */ @@ -626,7 +565,7 @@ MulticopterAttitudeControl::task_main() if (publish_att_sp) { /* publish the attitude setpoint */ _att_sp.timestamp = hrt_absolute_time(); -printf("att_sp RPYT: %.2f %.2f %.2f %.2f", _att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body, _att_sp.thrust) + if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); -- cgit v1.2.3 From 0877fcba0312cf859b2a8ccd9eb3b6d56ecaf7e4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 17:52:02 +0100 Subject: offboard setpoint support --- src/modules/commander/commander.cpp | 213 ++++++++++++++++++------------- src/modules/mavlink/mavlink_receiver.cpp | 48 +++++-- src/modules/navigator/navigator_main.cpp | 57 ++++++++- 3 files changed, 215 insertions(+), 103 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7b69f2b76..fa815bdfe 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -119,6 +119,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ #define RC_TIMEOUT 100000 +#define OFFBOARD_TIMEOUT 200000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -699,9 +700,9 @@ int commander_thread_main(int argc, char *argv[]) memset(&sp_man, 0, sizeof(sp_man)); /* Subscribe to offboard control data */ - int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s sp_offboard; - memset(&sp_offboard, 0, sizeof(sp_offboard)); + int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s offboard_sp; + memset(&offboard_sp, 0, sizeof(offboard_sp)); /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -815,10 +816,10 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - orb_check(sp_offboard_sub, &updated); + orb_check(offboard_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp); } orb_check(sensor_sub, &updated); @@ -1040,124 +1041,162 @@ int commander_thread_main(int argc, char *argv[]) } } - /* ignore RC signals if in offboard control mode */ - if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* start RC input check */ - if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { - /* handle the case where RC signal was regained */ - if (!status.rc_signal_found_once) { - status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); - status_changed = true; + /* start RC input check */ + if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + status_changed = true; - } else { - if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); - status_changed = true; - } + } else { + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); + status_changed = true; } + } - status.rc_signal_lost = false; - - transition_result_t res; // store all transitions results here + status.rc_signal_lost = false; - /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + transition_result_t res; // store all transitions results here - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm - * do it only for rotary wings */ - if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* arm/disarm by RC */ + res = TRANSITION_NOT_CHANGED; - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); - stick_off_counter = 0; + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm + * do it only for rotary wings */ + if (status.is_rotary_wing && + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - } else { - stick_off_counter++; - } + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); + stick_off_counter = 0; } else { - stick_off_counter = 0; + stick_off_counter++; } - /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off) { - print_reject_arm("NOT ARMING: Press safety switch first."); - - } else if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + } else { + stick_off_counter = 0; + } - } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - } + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); - stick_on_counter = 0; + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - stick_on_counter++; + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } - } else { stick_on_counter = 0; + + } else { + stick_on_counter++; } - if (res == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + } else { + stick_on_counter = 0; + } - } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); - } + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); - } else if (res == TRANSITION_DENIED) { - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - /* fill current_status according to mode switches */ - check_mode_switches(&sp_man, &status); + } else if (res == TRANSITION_DENIED) { + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } + + /* fill current_status according to mode switches */ + check_mode_switches(&sp_man, &status); - /* evaluate the main state machine */ - res = check_main_state_machine(&status); + /* evaluate the main state machine */ + res = check_main_state_machine(&status); - if (res == TRANSITION_CHANGED) { - //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); - tune_positive(); + if (res == TRANSITION_CHANGED) { + //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + tune_positive(); - } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } + + } else { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + status.rc_signal_lost = true; + status_changed = true; + } + if (status.main_state != MAIN_STATE_AUTO && armed.armed) { + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + } else if (status.main_state != MAIN_STATE_SEATBELT) { + res = main_state_transition(&status, MAIN_STATE_SEATBELT); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); + } } + } + } + + /* check offboard signal */ + if (hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) { + if (!status.offboard_control_signal_found_once) { + status.offboard_control_signal_found_once = true; + mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time"); + status_changed = true; } else { - if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); - status.rc_signal_lost = true; + if (status.offboard_control_signal_lost) { + mavlink_log_info(mavlink_fd, "[cmd] offboard signal regained"); status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && armed.armed) { - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (status.main_state != MAIN_STATE_SEATBELT) { - res = main_state_transition(&status, MAIN_STATE_SEATBELT); + } + + status.offboard_control_signal_lost = false; + + if (status.main_state == MAIN_STATE_OFFBOARD) { + if (offboard_sp.armed && !armed.armed) { + if (!safety.safety_switch_available || safety.safety_off) { + transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); + mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); } } + } else if (!offboard_sp.armed && armed.armed) { + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); + if (res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal"); + } } } + + } else { + if (!status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd[ CRITICAL: OFFBOARD SIGNAL LOST"); + status.offboard_control_signal_lost = true; + status_changed = true; + } } /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ @@ -1264,7 +1303,7 @@ int commander_thread_main(int argc, char *argv[]) led_deinit(); buzzer_deinit(); close(sp_man_sub); - close(sp_offboard_sub); + close(offboard_sp_sub); close(local_position_sub); close(global_position_sub); close(gps_sub); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4f9c718d2..62597d91a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -97,6 +97,7 @@ static mavlink_status_t status; static struct vehicle_vicon_position_s vicon_position; static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; +static struct vehicle_attitude_setpoint_s att_sp; struct vehicle_global_position_s hil_global_pos; struct vehicle_local_position_s hil_local_pos; @@ -125,6 +126,7 @@ static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; static orb_advert_t offboard_control_sp_pub = -1; +static orb_advert_t att_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; static orb_advert_t telemetry_status_pub = -1; @@ -266,8 +268,8 @@ handle_message(mavlink_message_t *msg) /* Handle quadrotor motor setpoints */ if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_swarm_setpoint; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_swarm_setpoint); if (mavlink_system.sysid < 4) { @@ -281,7 +283,7 @@ handle_message(mavlink_message_t *msg) uint8_t ml_mode = 0; bool ml_armed = false; - switch (quad_motors_setpoint.mode) { + switch (quad_swarm_setpoint.mode) { case 0: ml_armed = false; break; @@ -307,12 +309,12 @@ handle_message(mavlink_message_t *msg) break; } - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; + offboard_control_sp.p1 = (float)quad_swarm_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_swarm_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_swarm_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_swarm_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { + if (quad_swarm_setpoint.thrust[mavlink_system.sysid - 1] == 0) { ml_armed = false; } @@ -326,9 +328,37 @@ handle_message(mavlink_message_t *msg) offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); } else { - /* Publish */ + /* publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } + + if (v_status.main_state == MAIN_STATE_OFFBOARD) { + /* in offboard mode also publish setpoint directly */ + switch (offboard_control_sp.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = offboard_control_sp.p1; + att_sp.pitch_body = offboard_control_sp.p2; + att_sp.yaw_body = offboard_control_sp.p3; + att_sp.thrust = offboard_control_sp.p4; + att_sp.R_valid = false; + att_sp.q_d_valid = false; + att_sp.q_e_valid = false; + att_sp.roll_reset_integral = false; + + /* check if topic has to be advertised */ + if (att_sp_pub <= 0) { + att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + + } else { + /* publish */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + break; + default: + break; + } + } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7285e243e..f77c48207 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -149,8 +150,9 @@ private: int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _offboard_mission_sub; /**< notification of offboard mission updates */ - int _onboard_mission_sub; /**< notification of onboard mission updates */ + int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _offboard_sub; /**< offboard control setpoint subscription */ orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ @@ -162,6 +164,7 @@ private: struct home_position_s _home_pos; /**< home position for RTL */ struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ + struct offboard_control_setpoint_s _offboard; /**< offboard control setpoint, to get desired offboard control mode only */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -383,6 +386,7 @@ Navigator::Navigator() : _offboard_mission_sub(-1), _onboard_mission_sub(-1), _capabilities_sub(-1), + _offboard_sub(-1), /* publications */ _triplet_pub(-1), @@ -422,6 +426,7 @@ Navigator::Navigator() : memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s)); memset(&_mission_result, 0, sizeof(struct mission_result_s)); + memset(&_offboard, 0, sizeof(_offboard)); nav_states_str[0] = "NONE"; nav_states_str[1] = "READY"; @@ -591,6 +596,7 @@ Navigator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); + _offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); @@ -1523,12 +1529,49 @@ Navigator::publish_control_mode() case MAIN_STATE_OFFBOARD: _control_mode.flag_control_offboard_enabled = true; _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + + switch (_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = false; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = true; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + default: + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + } break; default: -- cgit v1.2.3 From 3fe5e88fbe291619783d7bc271a5152d4e42bd3f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 17:53:55 +0100 Subject: Reverse offboard changes in mc_att_control --- src/modules/mc_att_control/mc_att_control_main.cpp | 99 ++++++++++------------ 1 file changed, 45 insertions(+), 54 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c4c6c0138..245ac024b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -480,73 +480,64 @@ MulticopterAttitudeControl::task_main() float yaw_sp_move_rate = 0.0f; bool publish_att_sp = false; - if (_control_mode.flag_control_offboard_enabled) { - /* offboard control */ - // TODO set _att_sp or _rates_sp here depending on offboard control mode - // TODO _control_mode.flag_control_XXX flags are all false, set it according to ofboard control mode - // but it's not very beautiful, need to think how to do it better - - } else { - /* onboard control */ - /* define which input is the dominating control input */ - if (_control_mode.flag_control_manual_enabled) { - /* manual input */ - if (!_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - _att_sp.thrust = _manual.throttle; - } + /* define which input is the dominating control input */ + if (_control_mode.flag_control_manual_enabled) { + /* manual input */ + if (!_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + _att_sp.thrust = _manual.throttle; + } - if (!_arming.armed) { - /* reset yaw setpoint when disarmed */ - reset_yaw_sp = true; - } + if (!_arming.armed) { + /* reset yaw setpoint when disarmed */ + reset_yaw_sp = true; + } - if (_control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - - if (_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual.yaw; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); - _att_sp.R_valid = false; - publish_att_sp = true; - } - } + if (_control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; + if (_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual.yaw; + _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); _att_sp.R_valid = false; publish_att_sp = true; } + } - if (!_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _att_sp.roll_body = _manual.roll; - _att_sp.pitch_body = _manual.pitch; - _att_sp.R_valid = false; - publish_att_sp = true; - } + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp.yaw_body = _att.yaw; + _att_sp.R_valid = false; + publish_att_sp = true; + } - } else { - /* manual rate inputs (ACRO) */ - // TODO - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; + if (!_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _att_sp.roll_body = _manual.roll; + _att_sp.pitch_body = _manual.pitch; + _att_sp.R_valid = false; + publish_att_sp = true; } } else { - /* reset yaw setpoint after non-manual control */ + /* manual rate inputs (ACRO) */ + // TODO + /* reset yaw setpoint after ACRO */ reset_yaw_sp = true; } + + } else { + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; } if (_att_sp.R_valid) { -- cgit v1.2.3 From 6ed6c5bb1fe6cce2c2c784d5f8145c0aa03fabe7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 17:56:38 +0100 Subject: cammander, navigator: code style fixed --- src/modules/commander/commander.cpp | 26 +- src/modules/commander/state_machine_helper.cpp | 56 ++-- src/modules/navigator/navigator_main.cpp | 352 +++++++++++++++---------- 3 files changed, 258 insertions(+), 176 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fa815bdfe..bc8278bb7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -372,6 +372,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } } + if (hil_ret == OK) ret = true; @@ -406,6 +407,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; } } + if (arming_res == TRANSITION_CHANGED) ret = true; @@ -454,6 +456,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } } + } else { mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd"); } @@ -497,8 +500,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_OVERRIDE_GOTO: { - // TODO listen vehicle_command topic directly from navigator (?) + // TODO listen vehicle_command topic directly from navigator (?) unsigned int mav_goto = cmd->param1; + if (mav_goto == 0) { // MAV_GOTO_DO_HOLD status->set_nav_state = NAV_STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); @@ -519,7 +523,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO @@ -891,6 +895,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); + /* only consider battery voltage if system has been running 2s and battery voltage is valid */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; @@ -1066,9 +1071,9 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1086,7 +1091,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off) { print_reject_arm("NOT ARMING: Press safety switch first."); @@ -1143,14 +1148,18 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; } + if (status.main_state != MAIN_STATE_AUTO && armed.armed) { transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); status.set_nav_state = NAV_STATE_RTL; status.set_nav_state_timestamp = hrt_absolute_time(); + } else if (status.main_state != MAIN_STATE_SEATBELT) { res = main_state_transition(&status, MAIN_STATE_SEATBELT); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); } @@ -1178,13 +1187,16 @@ int commander_thread_main(int argc, char *argv[]) if (offboard_sp.armed && !armed.armed) { if (!safety.safety_switch_available || safety.safety_off) { transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + if (res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); } } + } else if (!offboard_sp.armed && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); + if (res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal"); } @@ -1202,9 +1214,11 @@ int commander_thread_main(int argc, char *argv[]) /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); + if (flighttermination_res == TRANSITION_CHANGED) { tune_positive(); } + } else { flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e5599e1c3..4dd63b5e1 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,7 @@ static bool flighttermination_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed) + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -85,6 +85,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; + } else { armed->lockdown = false; } @@ -238,8 +239,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m /* need at minimum altitude estimate */ if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { + (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } @@ -249,7 +250,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m /* need at minimum local position estimate */ if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { + current_state->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -373,35 +374,36 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s * { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->flighttermination_state) { - ret = TRANSITION_NOT_CHANGED; + /* only check transition if the new state is actually different from the current one */ + if (new_flighttermination_state == status->flighttermination_state) { + ret = TRANSITION_NOT_CHANGED; - } else { + } else { - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; - warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); - break; - case FLIGHTTERMINATION_STATE_OFF: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; - break; + switch (new_flighttermination_state) { + case FLIGHTTERMINATION_STATE_ON: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; + warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); + break; - default: - break; - } + case FLIGHTTERMINATION_STATE_OFF: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; + break; - if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; - } + default: + break; } - return ret; + if (ret == TRANSITION_CHANGED) { + flighttermination_state_changed = true; + // TODO + //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + } + } + + return ret; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f77c48207..0e317d784 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -167,12 +167,12 @@ private: struct offboard_control_setpoint_s _offboard; /**< offboard control setpoint, to get desired offboard control mode only */ perf_counter_t _loop_perf; /**< loop performance counter */ - + Geofence _geofence; bool _geofence_violation_warning_sent; bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ + bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; @@ -369,7 +369,7 @@ static const int ERROR = -1; Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), @@ -508,16 +508,20 @@ void Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); @@ -534,6 +538,7 @@ void Navigator::onboard_mission_update() { struct mission_s onboard_mission; + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -576,11 +581,13 @@ Navigator::task_main() * else clear geofence data in datamanager */ struct stat buffer; - if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) { + + if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); + } else { - if (_geofence.clearDm() > 0 ) + if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); @@ -597,7 +604,7 @@ Navigator::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - + /* copy all topics first time */ vehicle_status_update(); parameters_update(); @@ -665,8 +672,9 @@ Navigator::task_main() /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { + (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ @@ -674,21 +682,27 @@ Navigator::task_main() if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } + stick_mode = true; + } else { /* MISSION switch */ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { dispatch(EVENT_LOITER_REQUESTED); stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { /* switch to mission only if available */ if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } + stick_mode = true; } + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ dispatch(EVENT_LOITER_REQUESTED); @@ -714,15 +728,18 @@ Navigator::task_main() case NAV_STATE_MISSION: if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } + break; case NAV_STATE_RTL: if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } + break; default: @@ -735,6 +752,7 @@ Navigator::task_main() if (myState == NAV_STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } @@ -762,7 +780,7 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } @@ -783,6 +801,7 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { @@ -791,15 +810,15 @@ Navigator::task_main() } /* Check geofence violation */ - if(!_geofence.inside(&_global_pos)) { + if (!_geofence.inside(&_global_pos)) { //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ - if (!_geofence_violation_warning_sent) - { + if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } + } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; @@ -849,48 +868,55 @@ Navigator::start() } void -Navigator::status() +Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + if (_global_pos.valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)_global_pos.relative_alt); + (double)_global_pos.alt, (double)_global_pos.relative_alt); warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", - (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } + if (_fence_valid) { warnx("Geofence is valid"); // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else { warnx("Geofence not set"); } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); - break; - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - default: - warnx("State: Unknown"); - break; + case NAV_STATE_NONE: + warnx("State: None"); + break; + + case NAV_STATE_LOITER: + warnx("State: Loiter"); + break; + + case NAV_STATE_MISSION: + warnx("State: Mission"); + break; + + case NAV_STATE_RTL: + warnx("State: RTL"); + break; + + default: + warnx("State: Unknown"); + break; } } StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* STATE_NONE */ + { + /* STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -899,8 +925,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, - { - /* STATE_READY */ + { + /* STATE_READY */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, @@ -910,7 +936,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, { - /* STATE_LOITER */ + /* STATE_LOITER */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, @@ -919,8 +945,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, - { - /* STATE_MISSION */ + { + /* STATE_MISSION */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -929,8 +955,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, - { - /* STATE_RTL */ + { + /* STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -993,6 +1019,7 @@ Navigator::start_loiter() if (_global_pos.alt < min_alt_amsl) { _mission_item_triplet.current.altitude = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + } else { _mission_item_triplet.current.altitude = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); @@ -1043,27 +1070,28 @@ Navigator::set_mission_item() add_home_pos_to_rtl(&_mission_item_triplet.current); if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { /* don't reset RTL state on RTL or LOITER items */ _rtl_state = RTL_STATE_NONE; } if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( - _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { + _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED + )) { /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; /* calculate desired takeoff altitude AMSL */ float takeoff_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) takeoff_alt_amsl += _home_pos.altitude; @@ -1089,6 +1117,7 @@ Navigator::set_mission_item() _mission_item_triplet.current.altitude = takeoff_alt_amsl; _mission_item_triplet.current.altitude_is_relative = false; } + } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; @@ -1097,13 +1126,16 @@ Navigator::set_mission_item() if (_do_takeoff) { mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude); + } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); } } + } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ _mission_item_triplet.current_valid = false; @@ -1116,6 +1148,7 @@ Navigator::set_mission_item() if (ret == OK) { add_home_pos_to_rtl(&_mission_item_triplet.next); _mission_item_triplet.next_valid = true; + } else { /* this will fail for the last WP */ _mission_item_triplet.next_valid = false; @@ -1129,17 +1162,21 @@ void Navigator::start_rtl() { _do_takeoff = false; + if (_rtl_state == RTL_STATE_NONE) { if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; + } else { _rtl_state = RTL_STATE_RETURN; + if (_reset_loiter_pos) { _mission_item_triplet.current.altitude_is_relative = false; _mission_item_triplet.current.altitude = _global_pos.alt; } } } + _reset_loiter_pos = true; mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); set_rtl_item(); @@ -1150,107 +1187,112 @@ Navigator::set_rtl_item() { switch (_rtl_state) { case RTL_STATE_CLIMB: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - float climb_alt = _home_pos.altitude + _parameters.rtl_alt; - if (_vstatus.condition_landed) - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + float climb_alt = _home_pos.altitude + _parameters.rtl_alt; + + if (_vstatus.condition_landed) + climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _mission_item_triplet.current.altitude = climb_alt; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = climb_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); - break; - } case RTL_STATE_RETURN: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + // don't change altitude setpoint + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + break; + } - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - // don't change altitude setpoint - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); - break; - } case RTL_STATE_DESCEND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - float descend_alt = _home_pos.altitude + _parameters.land_alt; + float descend_alt = _home_pos.altitude + _parameters.land_alt; + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = descend_alt; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = descend_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); - break; - } case RTL_STATE_LAND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } default: { - mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } + mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + start_loiter(); + break; + } } publish_mission_item_triplet(); @@ -1267,6 +1309,7 @@ Navigator::check_mission_item_reached() if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { if (_vstatus.is_rotary_wing) { return _vstatus.condition_landed; + } else { /* For fw there is currently no landing detector: * make sure control is not stopped when overshooting the landing waypoint */ @@ -1281,7 +1324,7 @@ Navigator::check_mission_item_reached() _mission_item_triplet.current.loiter_radius > 0.01f) { return false; - } + } uint64_t now = hrt_absolute_time(); @@ -1301,18 +1344,20 @@ Navigator::check_mission_item_reached() /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float wp_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) _mission_item_triplet.current.altitude += _home_pos.altitude; dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, - &dist_xy, &dist_z); + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, + &dist_xy, &dist_z); if (_do_takeoff) { if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { /* require only altitude for takeoff */ _waypoint_position_reached = true; } + } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1324,9 +1369,11 @@ Navigator::check_mission_item_reached() if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } + } else { _waypoint_yaw_reached = true; } @@ -1336,20 +1383,22 @@ Navigator::check_mission_item_reached() if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; + if (_mission_item_triplet.current.time_inside > 0.01f) { mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside); } } - + /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) - || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; return true; } } + return false; } @@ -1362,6 +1411,7 @@ Navigator::on_mission_item_reached() /* takeoff completed */ _do_takeoff = false; mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + } else { /* advance by one mission item */ _mission.move_to_next(); @@ -1369,23 +1419,28 @@ Navigator::on_mission_item_reached() if (_mission.current_mission_available()) { set_mission_item(); + } else { /* if no more mission items available then finish mission */ /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); + if (_vstatus.condition_landed) { dispatch(EVENT_READY_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } } + } else { /* RTL finished */ if (_rtl_state == RTL_STATE_LAND) { /* landed at home position */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); dispatch(EVENT_READY_REQUESTED); + } else { /* next RTL step */ _rtl_state = (RTLState)(_rtl_state + 1); @@ -1508,6 +1563,7 @@ Navigator::publish_control_mode() case MAIN_STATE_AUTO: _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = false; + if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; @@ -1516,6 +1572,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; + } else { _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1524,6 +1581,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; } + break; case MAIN_STATE_OFFBOARD: @@ -1539,6 +1597,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1547,6 +1606,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1555,6 +1615,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = true; break; + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1563,6 +1624,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_position_enabled = true; _control_mode.flag_control_velocity_enabled = true; break; + default: _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1572,6 +1634,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; break; } + break; default: @@ -1591,7 +1654,8 @@ Navigator::publish_control_mode() } } -bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { +bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) +{ if (a.altitude_is_relative == b.altitude_is_relative && fabs(a.lat - b.lat) < FLT_EPSILON && fabs(a.lon - b.lon) < FLT_EPSILON && @@ -1604,6 +1668,7 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && a.autocontinue == b.autocontinue) { return true; + } else { return false; } @@ -1664,8 +1729,9 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { - navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); + navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); } else { usage(); -- cgit v1.2.3 From dfedbcb855e53da5fe2ce4d37d9f4fba54626d62 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Fri, 24 Jan 2014 23:56:54 +0000 Subject: added check for MAIN_STATE_OFFBOARD @#1152 so that we dont get thrown out of this state --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bc8278bb7..bec6884e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1149,7 +1149,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && armed.armed) { + if (status.main_state != MAIN_STATE_AUTO && armed.armed && status.main_state != MAIN_STATE_OFFBOARD) { transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); if (res == TRANSITION_CHANGED) { -- cgit v1.2.3 From a037861ec21834428416e92482feacad749e1cd3 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Mon, 27 Jan 2014 14:33:34 +0000 Subject: Added method to update _offboard in navigator_main.cpp --- src/modules/navigator/navigator_main.cpp | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0e317d784..829511053 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -271,6 +271,11 @@ private: * Retrieve vehicle status */ void vehicle_status_update(); + + /** + * Retrieve offboard setpoint + */ + void offboard_update(); /** @@ -503,6 +508,11 @@ Navigator::navigation_capabilities_update() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } +void +Navigator::offboard_update() +{ + orb_copy(ORB_ID(offboard_control_setpoint), _offboard_sub, &_offboard); +} void Navigator::offboard_mission_update(bool isrotaryWing) @@ -613,6 +623,7 @@ Navigator::task_main() navigation_capabilities_update(); offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); + offboard_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -623,7 +634,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -640,6 +651,9 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _vstatus_sub; fds[6].events = POLLIN; + fds[7].fd = _offboard_sub; + fds[7].events = POLLIN; + while (!_task_should_exit) { @@ -825,6 +839,11 @@ Navigator::task_main() } } + /* offboard setpoint updated */ + if (fds[7].revents & POLLIN) { + offboard_update(); + } + /* notify user about state changes */ if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s -> %s", nav_states_str[prevState], nav_states_str[myState]); -- cgit v1.2.3 From 59a5f37b7feb0cecfdd983e88ce55931f3c1bce1 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 29 Jan 2014 15:39:58 +0000 Subject: Added check for offboard_control_signal_lost when switching to offboard mode with rc switch --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bec6884e2..3687f55fb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1478,8 +1478,8 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta if (!isfinite(sp_man->offboard_switch)) { current_status->offboard_switch = OFFBOARD_SWITCH_NONE; - } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { - current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT && !status.offboard_control_signal_lost) { + current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; } else { current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; -- cgit v1.2.3 From 01c9092213449c761759bcda11ef9613226be713 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 18:06:03 +0100 Subject: Revert "Added check for offboard_control_signal_lost when switching to offboard mode with rc switch" This reverts commit 59a5f37b7feb0cecfdd983e88ce55931f3c1bce1. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3687f55fb..bec6884e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1478,8 +1478,8 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta if (!isfinite(sp_man->offboard_switch)) { current_status->offboard_switch = OFFBOARD_SWITCH_NONE; - } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT && !status.offboard_control_signal_lost) { - current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { + current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; } else { current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; -- cgit v1.2.3 From 8ab94150db31e270b34cecaa4701628df873042c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Feb 2014 20:23:23 +0100 Subject: commander: print_reject_mode() bug fixed --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 246efd61e..2c2b680b4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1624,7 +1624,7 @@ set_main_state_rc(struct vehicle_status_s *status) if (status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { res = main_state_transition(status, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { - print_reject_mode("OFFBOARD"); + print_reject_mode(status, "OFFBOARD"); } else { return res; -- cgit v1.2.3 From 15ea9cd401351f7487cff35333780d6d4ff47e09 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Fri, 14 Feb 2014 12:37:10 +0000 Subject: minor cosmetic changes --- src/modules/commander/commander.cpp | 1 - src/modules/mavlink/mavlink_receiver.cpp | 10 ++++------ 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3687f55fb..3e5d9dcf6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1491,7 +1491,6 @@ check_main_state_machine(struct vehicle_status_s *current_status) { /* evaluate the main state machine */ transition_result_t res = TRANSITION_DENIED; - if (current_status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { /* offboard switch overrides main switch */ res = main_state_transition(current_status, MAIN_STATE_OFFBOARD); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 62597d91a..f3345b35f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -288,23 +288,21 @@ handle_message(mavlink_message_t *msg) ml_armed = false; break; - case 1: + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; ml_armed = true; - break; - case 2: + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; ml_armed = true; - break; - case 3: + case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; break; - case 4: + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; break; } -- cgit v1.2.3 From 66f270dd7e2a8668ae7a4f3387e5098381eff84d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 14 Feb 2014 13:39:57 +0100 Subject: commander: double subscribing to offboard setpoint fixed --- src/modules/commander/commander.cpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2c2b680b4..bf7bf2ef3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -761,11 +761,6 @@ int commander_thread_main(int argc, char *argv[]) struct manual_control_setpoint_s sp_man; memset(&sp_man, 0, sizeof(sp_man)); - /* Subscribe to offboard control data */ - int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s offboard_sp; - memset(&offboard_sp, 0, sizeof(offboard_sp)); - /* Subscribe to offboard control data */ int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); struct offboard_control_setpoint_s sp_offboard; @@ -883,10 +878,6 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp); - } - orb_check(sp_offboard_sub, &updated); if (updated) { @@ -1285,7 +1276,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check offboard signal */ - if (offboard_sp.timestamp != 0 && hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) { + if (sp_offboard.timestamp != 0 && hrt_absolute_time() < sp_offboard.timestamp + OFFBOARD_TIMEOUT) { if (!status.offboard_control_signal_found_once) { status.offboard_control_signal_found_once = true; mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time"); @@ -1301,7 +1292,7 @@ int commander_thread_main(int argc, char *argv[]) status.offboard_control_signal_lost = false; if (status.main_state == MAIN_STATE_OFFBOARD) { - if (offboard_sp.armed && !armed.armed) { + if (sp_offboard.armed && !armed.armed) { if (!safety.safety_switch_available || safety.safety_off) { transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); @@ -1310,7 +1301,7 @@ int commander_thread_main(int argc, char *argv[]) } } - } else if (!offboard_sp.armed && armed.armed) { + } else if (!sp_offboard.armed && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); -- cgit v1.2.3 From 08055e5d52b8c522ca86e86bc161343dc396e5d9 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Mon, 17 Feb 2014 11:45:51 +0000 Subject: added examples/jetdrive_control(copy of mc_att_control) --- .gitignore | 6 + makefiles/config_px4fmu-v2_default.mk | 1 + src/examples/jetdrive_control/jetdrive_control.h | 207 +++++++ .../jetdrive_control/jetdrive_control_main.cpp | 678 +++++++++++++++++++++ .../jetdrive_control/jetdrive_control_params.c | 56 ++ src/examples/jetdrive_control/module.mk | 41 ++ 6 files changed, 989 insertions(+) create mode 100644 src/examples/jetdrive_control/jetdrive_control.h create mode 100644 src/examples/jetdrive_control/jetdrive_control_main.cpp create mode 100644 src/examples/jetdrive_control/jetdrive_control_params.c create mode 100644 src/examples/jetdrive_control/module.mk diff --git a/.gitignore b/.gitignore index 3e94cf620..63f0a70b7 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,9 @@ mavlink/include/mavlink/v0.9/ /Documentation/doxygen*objdb*tmp .tags .tags_sorted_by_file +*.creator +*.includes +*.files +*.config +*.user +*.autosave diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ad2f63a9..ca740a79f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -141,6 +141,7 @@ MODULES += examples/px4_simple_app # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control #MODULES += examples/fixedwing_control +MODULES += examples/jetdrive_control # Hardware test #MODULES += examples/hwtest diff --git a/src/examples/jetdrive_control/jetdrive_control.h b/src/examples/jetdrive_control/jetdrive_control.h new file mode 100644 index 000000000..06c986092 --- /dev/null +++ b/src/examples/jetdrive_control/jetdrive_control.h @@ -0,0 +1,207 @@ +#ifndef JETDRIVE_CONTROL_H +#define JETDRIVE_CONTROL_H + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int jetdrive_control_main(int argc, char *argv[]); + +#define MIN_TAKEOFF_THROTTLE 0.3f +#define YAW_DEADZONE 0.05f +#define RATES_I_LIMIT 0.5f + +class JetdriveControl +{ +public: + /** + * Constructor + */ + JetdriveControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~JetdriveControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _v_att_sub; /**< vehicle attitude subscription */ + int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ + int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ + int _v_control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< parameter updates subscription */ + int _manual_control_sp_sub; /**< manual control setpoint subscription */ + int _armed_sub; /**< arming status subscription */ + + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ + struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ + struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator controls */ + struct actuator_armed_s _armed; /**< actuator arming status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */ + math::Matrix<3, 3> _R; /**< rotation matrix for current state */ + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp; /**< angular rates setpoint */ + math::Vector<3> _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + math::Vector<3> _att_control; /**< attitude control vector */ + + math::Matrix<3, 3> I; /**< identity matrix */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ + + struct { + param_t roll_p; + param_t roll_rate_p; + param_t roll_rate_i; + param_t roll_rate_d; + param_t pitch_p; + param_t pitch_rate_p; + param_t pitch_rate_i; + param_t pitch_rate_d; + param_t yaw_p; + param_t yaw_rate_p; + param_t yaw_rate_i; + param_t yaw_rate_d; + param_t yaw_ff; + + param_t rc_scale_yaw; + } _params_handles; /**< handles for interesting parameters */ + + struct { + math::Vector<3> att_p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + + float rc_scale_yaw; + } _params; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Check for parameter update and handle it. + */ + void parameter_update_poll(); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + /** + * Check for attitude setpoint updates. + */ + void vehicle_attitude_setpoint_poll(); + + /** + * Check for rates setpoint updates. + */ + void vehicle_rates_setpoint_poll(); + + /** + * Check for arming status updates. + */ + void arming_status_poll(); + + /** + * Attitude controller. + */ + void control_attitude(float dt); + + /** + * Attitude rates controller. + */ + void control_attitude_rates(float dt); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + + +#ifndef jetdrive_control +#define jetdrive_control + +namespace jetdrive_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +JetdriveControl *g_control; +} + +#endif // jetdrive_control + +#endif // JETDRIVE_CONTROL_H diff --git a/src/examples/jetdrive_control/jetdrive_control_main.cpp b/src/examples/jetdrive_control/jetdrive_control_main.cpp new file mode 100644 index 000000000..b66b9c214 --- /dev/null +++ b/src/examples/jetdrive_control/jetdrive_control_main.cpp @@ -0,0 +1,678 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Author: @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_att_control_main.c + * Multicopter attitude controller. + * + * The controller has two loops: P loop for angular error and PD loop for angular rate error. + * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. + * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, + * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. + * These two approaches fused seamlessly with weight depending on angular error. + * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. + */ + +#include "jetdrive_control.h" + + + +JetdriveControl::JetdriveControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _v_att_sub(-1), + _v_att_sp_sub(-1), + _v_control_mode_sub(-1), + _params_sub(-1), + _manual_control_sp_sub(-1), + _armed_sub(-1), + +/* publications */ + _att_sp_pub(-1), + _v_rates_sp_pub(-1), + _actuators_0_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) + +{ + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_armed, 0, sizeof(_armed)); + + _params.att_p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + + _R_sp.identity(); + _R.identity(); + _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + I.identity(); + + _params_handles.roll_p = param_find("MC_ROLL_P"); + _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + + /* fetch initial parameter values */ + parameters_update(); +} + +JetdriveControl::~JetdriveControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + jetdrive_control::g_control = nullptr; +} + +int +JetdriveControl::parameters_update() +{ + float v; + + /* roll */ + param_get(_params_handles.roll_p, &v); + _params.att_p(0) = v; + param_get(_params_handles.roll_rate_p, &v); + _params.rate_p(0) = v; + param_get(_params_handles.roll_rate_i, &v); + _params.rate_i(0) = v; + param_get(_params_handles.roll_rate_d, &v); + _params.rate_d(0) = v; + + /* pitch */ + param_get(_params_handles.pitch_p, &v); + _params.att_p(1) = v; + param_get(_params_handles.pitch_rate_p, &v); + _params.rate_p(1) = v; + param_get(_params_handles.pitch_rate_i, &v); + _params.rate_i(1) = v; + param_get(_params_handles.pitch_rate_d, &v); + _params.rate_d(1) = v; + + /* yaw */ + param_get(_params_handles.yaw_p, &v); + _params.att_p(2) = v; + param_get(_params_handles.yaw_rate_p, &v); + _params.rate_p(2) = v; + param_get(_params_handles.yaw_rate_i, &v); + _params.rate_i(2) = v; + param_get(_params_handles.yaw_rate_d, &v); + _params.rate_d(2) = v; + + param_get(_params_handles.yaw_ff, &_params.yaw_ff); + + param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + + return OK; +} + +void +JetdriveControl::parameter_update_poll() +{ + bool updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_params_sub, &updated); + + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} + +void +JetdriveControl::vehicle_control_mode_poll() +{ + bool updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_v_control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); + } +} + +void +JetdriveControl::vehicle_manual_poll() +{ + bool updated; + + /* get pilots inputs */ + orb_check(_manual_control_sp_sub, &updated); + + if (updated) { + + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); + } +} + +void +JetdriveControl::vehicle_attitude_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); + } +} + +void +JetdriveControl::vehicle_rates_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_rates_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); + } +} + +void +JetdriveControl::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } +} + +/* + * Attitude controller. + * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) + * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) + */ +void +JetdriveControl::control_attitude(float dt) +{ + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; + + if (_v_control_mode.flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ + + if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + vehicle_attitude_setpoint_poll(); + } + + if (!_v_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp.thrust = _manual_control_sp.throttle; + publish_att_sp = true; + } + + if (!_armed.armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } + + /* move yaw setpoint in all modes */ + if (_v_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; + + if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; + + if (_manual_control_sp.yaw > 0.0f) { + yaw_sp_move_rate -= YAW_DEADZONE; + + } else { + yaw_sp_move_rate += YAW_DEADZONE; + } + + yaw_sp_move_rate *= _params.rc_scale_yaw; + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + } + + /* reset yaw setpint to current position if needed */ + if (_reset_yaw_sp) { + _reset_yaw_sp = false; + _v_att_sp.yaw_body = _v_att.yaw; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + if (!_v_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp.roll_body = _manual_control_sp.roll; + _v_att_sp.pitch_body = _manual_control_sp.pitch; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + vehicle_attitude_setpoint_poll(); + + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } + + _thrust_sp = _v_att_sp.thrust; + + /* construct attitude setpoint rotation matrix */ + if (_v_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + _R_sp.set(&_v_att_sp.R_body[0][0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + _R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body)); + _v_att_sp.R_valid = true; + } + + /* publish the attitude setpoint if needed */ + if (publish_att_sp) { + _v_att_sp.timestamp = hrt_absolute_time(); + + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + } + } + + /* rotation matrix for current state */ + _R.set(_v_att.R); + + /* all input data is ready, run controller itself */ + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2)); + math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = _R_sp(2, 2) * _R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = _R; + } + + /* R_rp and _R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0)); + math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + math::Quaternion q; + q.from_dcm(_R.transposed() * _R_sp); + math::Vector<3> e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* calculate angular rates setpoint */ + _rates_sp = _params.att_p.emult(e_R); + + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; +} + +/* + * Attitude rates controller. + * Input: '_rates_sp' vector, '_thrust_sp' + * Output: '_att_control' vector + */ +void +JetdriveControl::control_attitude_rates(float dt) +{ + /* reset integral if disarmed */ + if (!_armed.armed) { + _rates_int.zero(); + } + + /* current body angular rates */ + math::Vector<3> rates; + rates(0) = _v_att.rollspeed; + rates(1) = _v_att.pitchspeed; + rates(2) = _v_att.yawspeed; + + /* angular rates error */ + math::Vector<3> rates_err = _rates_sp - rates; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > 0.1f) { + for (int i = 0; i < 3; i++) { + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } + } + } + } +} + +void +JetdriveControl::task_main_trampoline(int argc, char *argv[]) +{ + jetdrive_control::g_control->task_main(); +} + +void +JetdriveControl::task_main() +{ + warnx("started"); + fflush(stdout); + + /* + * do subscriptions + */ + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + /* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */ + orb_set_interval(_v_att_sub, 5); + + /* initialize parameters cache */ + parameters_update(); + + /* wakeup source: vehicle attitude */ + struct pollfd fds[1]; + + fds[0].fd = _v_att_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(100000); + continue; + } + + perf_begin(_loop_perf); + + /* run controller on attitude changes */ + if (fds[0].revents & POLLIN) { + static uint64_t last_run = 0; + float dt = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too small (< 2ms) and too large (> 20ms) dt's */ + if (dt < 0.002f) { + dt = 0.002f; + + } else if (dt > 0.02f) { + dt = 0.02f; + } + + /* copy attitude topic */ + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + + /* check for updates in other topics */ + parameter_update_poll(); + vehicle_control_mode_poll(); + arming_status_poll(); + vehicle_manual_poll(); + + if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + } else { + /* attitude controller disabled */ + // TODO poll 'attitude_rates_setpoint' topic + _rates_sp.zero(); + _thrust_sp = 0.0f; + } + + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt); + + /* publish actuator controls */ + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.timestamp = hrt_absolute_time(); + + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + } + } + + perf_end(_loop_perf); + } + + warnx("exit"); + + _control_task = -1; + _exit(0); +} + +int +JetdriveControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("jetdrive_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&JetdriveControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int jetdrive_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: jetdrive_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (jetdrive_control::g_control != nullptr) + errx(1, "already running"); + + jetdrive_control::g_control = new JetdriveControl; + + if (jetdrive_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != jetdrive_control::g_control->start()) { + delete jetdrive_control::g_control; + jetdrive_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (jetdrive_control::g_control == nullptr) + errx(1, "not running"); + + delete jetdrive_control::g_control; + jetdrive_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (jetdrive_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/examples/jetdrive_control/jetdrive_control_params.c b/src/examples/jetdrive_control/jetdrive_control_params.c new file mode 100644 index 000000000..27a45b6bb --- /dev/null +++ b/src/examples/jetdrive_control/jetdrive_control_params.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Author: @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_att_control_params.c + * Parameters for multicopter attitude controller. + */ + +#include + +PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); diff --git a/src/examples/jetdrive_control/module.mk b/src/examples/jetdrive_control/module.mk new file mode 100644 index 000000000..2d1025451 --- /dev/null +++ b/src/examples/jetdrive_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Multirotor attitude controller (vector based, no Euler singularities) +# + +MODULE_COMMAND = jetdrive_control + +SRCS = jetdrive_control_main.cpp \ + jetdrive_control_params.c -- cgit v1.2.3 From e8bee868e2386b5ce9fc2bc4b8ca6aa63f240232 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 16:50:00 +0400 Subject: commander: publish vehicle_control_mode for OFFBOARD state --- src/modules/commander/commander.cpp | 32 ++++++++++++++++++++++---- src/modules/mavlink/mavlink_receiver.cpp | 1 + src/modules/uORB/topics/vehicle_control_mode.h | 1 + 3 files changed, 30 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1b9ca6988..61ef63921 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -159,6 +159,7 @@ static struct vehicle_status_s status; static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; +static struct offboard_control_setpoint_s sp_offboard; /* tasks waiting for low prio thread */ typedef enum { @@ -767,7 +768,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to offboard control data */ int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); /* Subscribe to global position */ @@ -1691,6 +1691,8 @@ set_control_mode() control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_offboard_enabled = false; control_mode.flag_control_termination_enabled = false; /* set this flag when navigator should act */ @@ -1701,7 +1703,6 @@ set_control_mode() switch (status.main_state) { case MAIN_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = status.is_rotary_wing; control_mode.flag_control_attitude_enabled = status.is_rotary_wing; control_mode.flag_control_altitude_enabled = false; @@ -1712,7 +1713,6 @@ set_control_mode() case MAIN_STATE_SEATBELT: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; @@ -1723,7 +1723,6 @@ set_control_mode() case MAIN_STATE_EASY: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; @@ -1735,6 +1734,31 @@ set_control_mode() case MAIN_STATE_AUTO: navigator_enabled = true; + case MAIN_STATE_OFFBOARD: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_offboard_enabled = false; + + switch (sp_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + default: + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + } + + break; + default: break; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0ca64853a..735bc50da 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -330,6 +330,7 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } + // TODO use vehicle_control_mode.flag_control_offboard_enabled if (v_status.main_state == MAIN_STATE_OFFBOARD) { /* in offboard mode also publish setpoint directly */ switch (offboard_control_sp.mode) { diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 7cbb37cd8..5444c4ebf 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -74,6 +74,7 @@ struct vehicle_control_mode_s bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ + bool flag_control_offboard_enabled; /**< true if offboard control should be used */ bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ -- cgit v1.2.3 From 172c82c258819c6f2d95b85b56c41015f928b282 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 17:21:43 +0400 Subject: commander: vehicle_control_mode fixes --- src/modules/commander/commander.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f82c8c35b..1ef872e39 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1736,7 +1736,7 @@ set_control_mode() case MAIN_STATE_OFFBOARD: control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_offboard_enabled = false; + control_mode.flag_control_offboard_enabled = true; switch (sp_offboard.mode) { case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: @@ -1776,7 +1776,6 @@ set_control_mode() case FAILSAFE_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = false; control_mode.flag_control_attitude_enabled = false; control_mode.flag_control_position_enabled = false; -- cgit v1.2.3 From 3b270fbdab60c0fac1c76dff170b0a9b6f494e8d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 18:07:12 +0400 Subject: commander: auto arm/disarm by offboard thrust "feature" removed --- src/modules/commander/commander.cpp | 20 -------------------- 1 file changed, 20 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1ef872e39..4a69318c9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1295,26 +1295,6 @@ int commander_thread_main(int argc, char *argv[]) status.offboard_control_signal_lost = false; - if (status.main_state == MAIN_STATE_OFFBOARD) { - if (sp_offboard.armed && !armed.armed) { - if (!safety.safety_switch_available || safety.safety_off) { - transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - - if (res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); - } - } - - } else if (!sp_offboard.armed && armed.armed) { - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); - - if (res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal"); - } - } - } - } else { if (!status.offboard_control_signal_lost) { mavlink_log_critical(mavlink_fd, "[cmd[ CRITICAL: OFFBOARD SIGNAL LOST"); -- cgit v1.2.3 From 9c20f86899b17dc6cd1e7642a6212e370474345e Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Tue, 18 Feb 2014 11:10:52 +0000 Subject: lalala --- src/modules/mavlink/mavlink_receiver.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 735bc50da..b724447c5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -330,7 +330,13 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } - // TODO use vehicle_control_mode.flag_control_offboard_enabled + static struct vehicle_control_mode_s _v_control_mode; + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + + /**< vehicle control mode subscription */ + static int _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + + // TODO use vehicle_control_mode.flag_control_offboard_enabled if (v_status.main_state == MAIN_STATE_OFFBOARD) { /* in offboard mode also publish setpoint directly */ switch (offboard_control_sp.mode) { -- cgit v1.2.3 From 92e15b83cc565cab149b7a8103794672dc82dcb5 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 11:24:32 +0000 Subject: added subscriber to vehicle_control_mode --- src/modules/mavlink/mavlink_receiver.cpp | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b724447c5..8ea0f9ea3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -98,6 +98,7 @@ static struct vehicle_vicon_position_s vicon_position; static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; static struct vehicle_attitude_setpoint_s att_sp; +static struct vehicle_control_mode_s _v_control_mode; struct vehicle_global_position_s hil_global_pos; struct vehicle_local_position_s hil_local_pos; @@ -117,6 +118,8 @@ static orb_advert_t pub_hil_baro = -1; static orb_advert_t pub_hil_airspeed = -1; static orb_advert_t pub_hil_battery = -1; +static int _v_control_mode_sub = -1; + /* packet counter */ static int hil_counter = 0; static int hil_frames = 0; @@ -330,14 +333,14 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } - static struct vehicle_control_mode_s _v_control_mode; - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - - /**< vehicle control mode subscription */ - static int _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - - // TODO use vehicle_control_mode.flag_control_offboard_enabled - if (v_status.main_state == MAIN_STATE_OFFBOARD) { + // TODO use vehicle_control_mode.flag_control_offboard_enabled + bool updated; + orb_check(_v_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); + } + if(_v_control_mode.flag_control_offboard_enabled) { +// if (v_status.main_state == MAIN_STATE_OFFBOARD) { /* in offboard mode also publish setpoint directly */ switch (offboard_control_sp.mode) { case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: @@ -863,6 +866,10 @@ receive_thread(void *arg) ssize_t nread = 0; + // vehicle control mode subscription + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { if (nread < sizeof(buf)) { -- cgit v1.2.3 From 1acd951173212fcb3d7511792270609b748528d7 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 11:26:15 +0000 Subject: added publish to actuators when _v_control_mode.flag_external_manual_override_ok flag is set --- .../jetdrive_control/jetdrive_control_main.cpp | 77 ++++++++++++++-------- 1 file changed, 48 insertions(+), 29 deletions(-) diff --git a/src/examples/jetdrive_control/jetdrive_control_main.cpp b/src/examples/jetdrive_control/jetdrive_control_main.cpp index b66b9c214..d91f89401 100644 --- a/src/examples/jetdrive_control/jetdrive_control_main.cpp +++ b/src/examples/jetdrive_control/jetdrive_control_main.cpp @@ -558,45 +558,64 @@ JetdriveControl::task_main() arming_status_poll(); vehicle_manual_poll(); - if (_v_control_mode.flag_control_attitude_enabled) { - control_attitude(dt); - - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); + if(_v_control_mode.flag_external_manual_override_ok) + { + /* manual/direct control */ + _actuators.control[0] = _manual_control_sp .roll; + _actuators.control[1] = _manual_control_sp.pitch; + _actuators.control[2] = _manual_control_sp.yaw; + _actuators.control[3] = _manual_control_sp.throttle; + _actuators.timestamp = hrt_absolute_time(); - if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } - } else { - /* attitude controller disabled */ - // TODO poll 'attitude_rates_setpoint' topic - _rates_sp.zero(); - _thrust_sp = 0.0f; - } + // FIX this: This is the atitude control logic for mc's - if (_v_control_mode.flag_control_rates_enabled) { - control_attitude_rates(dt); + if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); - /* publish actuator controls */ - _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; - _actuators.timestamp = hrt_absolute_time(); + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + /* attitude controller disabled */ + // TODO poll 'attitude_rates_setpoint' topic + _rates_sp.zero(); + _thrust_sp = 0.0f; + } + + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt); + + /* publish actuator controls */ + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.timestamp = hrt_absolute_time(); + + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } } } } -- cgit v1.2.3 From faa7d9577d20d38741fe9accabb372f088b9dc6a Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 11:27:42 +0000 Subject: commented out gcs_link = false; when receiving swarm messages, since we still want imu and atitude messages in offboard mode --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8ea0f9ea3..6c0eb169b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -277,7 +277,7 @@ handle_message(mavlink_message_t *msg) if (mavlink_system.sysid < 4) { /* switch to a receiving link mode */ - gcs_link = false; +// gcs_link = false; // commented this because I wanted to still receive imu and attitude info when in offboard mode /* * rate control mode - defined by MAVLink -- cgit v1.2.3 From f4a6b07e293037d632f080da4ff7504ff97731f6 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 11:29:31 +0000 Subject: indenting changes to make should_arm condition more readable --- src/modules/px4iofirmware/mixer.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6a4429461..760ca2e27 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -173,15 +173,16 @@ mixer_tick(void) * here. */ should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) - /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) - ) - ); + (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)/* IO initialised without error */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)/* and IO is armed */ + && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)/* and FMU is armed */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))/* and there is valid input via or mixer */ + || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)/* or direct PWM is set */ + || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))/* or failsafe was set manually */ + ) + ); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) -- cgit v1.2.3 From 52b29cae39a93a0c5218e911e61d1f6cfe6115c1 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 14:21:34 +0000 Subject: added poll to 'attitude_rates_setpoint' as requested in TODO --- src/examples/jetdrive_control/jetdrive_control_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/examples/jetdrive_control/jetdrive_control_main.cpp b/src/examples/jetdrive_control/jetdrive_control_main.cpp index d91f89401..409ccad63 100644 --- a/src/examples/jetdrive_control/jetdrive_control_main.cpp +++ b/src/examples/jetdrive_control/jetdrive_control_main.cpp @@ -596,8 +596,9 @@ JetdriveControl::task_main() } else { /* attitude controller disabled */ // TODO poll 'attitude_rates_setpoint' topic - _rates_sp.zero(); - _thrust_sp = 0.0f; + vehicle_rates_setpoint_poll(); +// _rates_sp.zero(); +// _thrust_sp = 0.0f; } if (_v_control_mode.flag_control_rates_enabled) { -- cgit v1.2.3 From 0656aae3cb46f405888d8130e9db4bb3f9dd529a Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 14:31:23 +0000 Subject: Now setting flag_external_manual_override_ok to false when in offboard mode,so that fmu has control over outputs. Added handling of OFFBOARD_CONTROL_MODE_DIRECT_RATES. --- src/modules/commander/commander.cpp | 11 ++++++++++- src/modules/mavlink/mavlink_receiver.cpp | 18 ++++++++++++++++++ 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f0debef1c..5e242050b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1728,8 +1728,17 @@ set_control_mode() control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; + control_mode.flag_external_manual_override_ok = false; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_external_manual_override_ok = false; break; - default: control_mode.flag_control_rates_enabled = false; control_mode.flag_control_attitude_enabled = false; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6c0eb169b..ba886e29e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -98,6 +98,7 @@ static struct vehicle_vicon_position_s vicon_position; static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; static struct vehicle_attitude_setpoint_s att_sp; +static struct vehicle_rates_setpoint_s rates_sp; static struct vehicle_control_mode_s _v_control_mode; struct vehicle_global_position_s hil_global_pos; @@ -130,6 +131,7 @@ static orb_advert_t flow_pub = -1; static orb_advert_t offboard_control_sp_pub = -1; static orb_advert_t att_sp_pub = -1; +static orb_advert_t rates_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; static orb_advert_t telemetry_status_pub = -1; @@ -343,6 +345,22 @@ handle_message(mavlink_message_t *msg) // if (v_status.main_state == MAIN_STATE_OFFBOARD) { /* in offboard mode also publish setpoint directly */ switch (offboard_control_sp.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + rates_sp.timestamp = hrt_absolute_time(); + rates_sp.roll = offboard_control_sp.p1; + rates_sp.pitch = offboard_control_sp.p2; + rates_sp.yaw = offboard_control_sp.p3; + rates_sp.thrust = offboard_control_sp.p4; + + /* check if topic has to be advertised */ + if(rates_sp_pub <= 0) { + rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + + } else { + /* publish */ + orb_publish(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } + break; case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: att_sp.timestamp = hrt_absolute_time(); att_sp.roll_body = offboard_control_sp.p1; -- cgit v1.2.3 From be4af0ab3bd1b4313ba23cbe4c7e2702d56d3644 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 17:07:29 +0000 Subject: code clean up --- src/examples/jetdrive_control/jetdrive_control_main.cpp | 2 +- src/modules/commander/commander.cpp | 2 -- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/examples/jetdrive_control/jetdrive_control_main.cpp b/src/examples/jetdrive_control/jetdrive_control_main.cpp index 409ccad63..01435f572 100644 --- a/src/examples/jetdrive_control/jetdrive_control_main.cpp +++ b/src/examples/jetdrive_control/jetdrive_control_main.cpp @@ -558,7 +558,7 @@ JetdriveControl::task_main() arming_status_poll(); vehicle_manual_poll(); - if(_v_control_mode.flag_external_manual_override_ok) + if(!_v_control_mode.flag_control_offboard_enabled) { /* manual/direct control */ _actuators.control[0] = _manual_control_sp .roll; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5e242050b..801ef8d01 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1728,7 +1728,6 @@ set_control_mode() control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; - control_mode.flag_external_manual_override_ok = false; break; case OFFBOARD_CONTROL_MODE_DIRECT_RATES: control_mode.flag_control_rates_enabled = true; @@ -1737,7 +1736,6 @@ set_control_mode() control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; - control_mode.flag_external_manual_override_ok = false; break; default: control_mode.flag_control_rates_enabled = false; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ba886e29e..a834b2c3a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -358,7 +358,7 @@ handle_message(mavlink_message_t *msg) } else { /* publish */ - orb_publish(ORB_ID(vehicle_rates_setpoint), &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } break; case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: -- cgit v1.2.3 From 0789189c0588ebebd24a523f9639411be89c6a9b Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Fri, 28 Feb 2014 16:24:15 +0000 Subject: - Added JC params to jetdrive control example app. - fixed offboard control mode rates --- .../jetdrive_control/jetdrive_control_main.cpp | 35 ++++++++++++++-------- .../jetdrive_control/jetdrive_control_params.c | 27 +++++++++-------- 2 files changed, 36 insertions(+), 26 deletions(-) diff --git a/src/examples/jetdrive_control/jetdrive_control_main.cpp b/src/examples/jetdrive_control/jetdrive_control_main.cpp index 01435f572..bb72f14e0 100644 --- a/src/examples/jetdrive_control/jetdrive_control_main.cpp +++ b/src/examples/jetdrive_control/jetdrive_control_main.cpp @@ -95,19 +95,19 @@ JetdriveControl::JetdriveControl() : I.identity(); - _params_handles.roll_p = param_find("MC_ROLL_P"); - _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); - _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); - _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); - _params_handles.pitch_p = param_find("MC_PITCH_P"); - _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); - _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); - _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); - _params_handles.yaw_p = param_find("MC_YAW_P"); - _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); - _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_p = param_find("JC_ROLL_P"); + _params_handles.roll_rate_p = param_find("JC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("JC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("JC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("JC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("JC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("JC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("JC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("JC_YAW_P"); + _params_handles.yaw_rate_p = param_find("JC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("JC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("JC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("JC_YAW_FF"); _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); @@ -463,6 +463,11 @@ JetdriveControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; + + // _att_control will be loaded to controls + // + // _att_control = (JC_XXXRATE_P * rates_err) + (JC_XXXRATE_D * (_rates_prev - rate) / dt) + _rates_int; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; _rates_prev = rates; @@ -597,6 +602,10 @@ JetdriveControl::task_main() /* attitude controller disabled */ // TODO poll 'attitude_rates_setpoint' topic vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = _v_rates_sp.thrust; // _rates_sp.zero(); // _thrust_sp = 0.0f; } diff --git a/src/examples/jetdrive_control/jetdrive_control_params.c b/src/examples/jetdrive_control/jetdrive_control_params.c index 27a45b6bb..14831ecf7 100644 --- a/src/examples/jetdrive_control/jetdrive_control_params.c +++ b/src/examples/jetdrive_control/jetdrive_control_params.c @@ -41,16 +41,17 @@ #include -PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); -PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); -PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); -PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); -PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); -PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); -PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); + +PARAM_DEFINE_FLOAT(JC_ROLL_P, 6.0f); +PARAM_DEFINE_FLOAT(JC_ROLLRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(JC_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(JC_ROLLRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(JC_PITCH_P, 6.0f); +PARAM_DEFINE_FLOAT(JC_PITCHRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(JC_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(JC_PITCHRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(JC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(JC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(JC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(JC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(JC_YAW_FF, 0.5f); -- cgit v1.2.3 From 313b546c4d2f098851dac2bdc88c2753b4377426 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 10 Mar 2014 23:37:26 +0100 Subject: fw pos ctrl: add wrapper function for tecs --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 96 ++++++++++++---------- 1 file changed, 53 insertions(+), 43 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f13df785..d6c706b3b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -356,6 +356,15 @@ private: * Reset landing state */ void reset_landing_state(); + + /* + * Call TECS + */ + void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad); + }; namespace l1_control @@ -819,11 +828,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { @@ -833,11 +841,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { @@ -929,11 +936,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_stayonground = true; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_pitch_angle_rad, - 0.0f, throttle_max, throttle_land, - flare_pitch_angle_rad, math::radians(15.0f)); + tecs_update_pitch_throttle(flare_curve_alt, calculate_target_airspeed(airspeed_land), eas2tas, + flare_pitch_angle_rad, math::radians(15.0f), + 0.0f, throttle_max, throttle_land, + false, flare_pitch_angle_rad); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -962,11 +968,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi altitude_desired = math::max(_global_pos.alt, L_altitude); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(altitude_desired, calculate_target_airspeed(airspeed_approach), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { @@ -1004,22 +1009,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), - _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f))); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); } else { - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } } else { @@ -1083,12 +1085,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, - _airspeed.indicated_airspeed_m_s, eas2tas, - false, _parameters.pitch_limit_min, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - _parameters.pitch_limit_min, _parameters.pitch_limit_max); + tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } else if (0/* seatbelt mode enabled */) { @@ -1127,12 +1127,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, - _airspeed.indicated_airspeed_m_s, eas2tas, - climb_out, _parameters.pitch_limit_min, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - _parameters.pitch_limit_min, _parameters.pitch_limit_max); + tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + climb_out, math::radians(_parameters.pitch_limit_min)); } else { @@ -1328,6 +1326,18 @@ void FixedwingPositionControl::reset_landing_state() land_onslope = false; } +void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad) +{ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, + _airspeed.indicated_airspeed_m_s, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); +} + int FixedwingPositionControl::start() { -- cgit v1.2.3 From 50d3fe2a1ace205f32ab9e236c3ddb1832d4f19c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 10 Mar 2014 23:37:26 +0100 Subject: fw pos ctrl: add wrapper function for tecs --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 96 ++++++++++++---------- 1 file changed, 53 insertions(+), 43 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f13df785..d6c706b3b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -356,6 +356,15 @@ private: * Reset landing state */ void reset_landing_state(); + + /* + * Call TECS + */ + void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad); + }; namespace l1_control @@ -819,11 +828,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { @@ -833,11 +841,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { @@ -929,11 +936,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_stayonground = true; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_pitch_angle_rad, - 0.0f, throttle_max, throttle_land, - flare_pitch_angle_rad, math::radians(15.0f)); + tecs_update_pitch_throttle(flare_curve_alt, calculate_target_airspeed(airspeed_land), eas2tas, + flare_pitch_angle_rad, math::radians(15.0f), + 0.0f, throttle_max, throttle_land, + false, flare_pitch_angle_rad); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -962,11 +968,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi altitude_desired = math::max(_global_pos.alt, L_altitude); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(altitude_desired, calculate_target_airspeed(airspeed_approach), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { @@ -1004,22 +1009,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), - _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f))); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); } else { - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } } else { @@ -1083,12 +1085,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, - _airspeed.indicated_airspeed_m_s, eas2tas, - false, _parameters.pitch_limit_min, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - _parameters.pitch_limit_min, _parameters.pitch_limit_max); + tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } else if (0/* seatbelt mode enabled */) { @@ -1127,12 +1127,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, - _airspeed.indicated_airspeed_m_s, eas2tas, - climb_out, _parameters.pitch_limit_min, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - _parameters.pitch_limit_min, _parameters.pitch_limit_max); + tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + climb_out, math::radians(_parameters.pitch_limit_min)); } else { @@ -1328,6 +1326,18 @@ void FixedwingPositionControl::reset_landing_state() land_onslope = false; } +void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad) +{ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, + _airspeed.indicated_airspeed_m_s, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); +} + int FixedwingPositionControl::start() { -- cgit v1.2.3 From 9c751fe9c5f604613a26b1d82e0735db44c9da3c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Mar 2014 00:05:57 +0100 Subject: mtecs --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 101 ++++++--- src/modules/fw_pos_control_l1/module.mk | 4 +- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 152 ++++++++++++++ src/modules/fw_pos_control_l1/mtecs/mTecs.h | 113 ++++++++++ src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 198 ++++++++++++++++++ src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 231 +++++++++++++++++++++ 6 files changed, 766 insertions(+), 33 deletions(-) create mode 100644 src/modules/fw_pos_control_l1/mtecs/mTecs.cpp create mode 100644 src/modules/fw_pos_control_l1/mtecs/mTecs.h create mode 100644 src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h create mode 100644 src/modules/fw_pos_control_l1/mtecs/mTecs_params.c diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d6c706b3b..1e8447aed 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -90,6 +90,7 @@ #include #include #include "landingslope.h" +#include "mtecs/mTecs.h" /** @@ -198,6 +199,8 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; + fwPosctrl::mTecs _mTecs; + bool _was_pos_control_mode; struct { float l1_period; @@ -331,11 +334,11 @@ private: /** * Control position. */ - bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, + bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -363,7 +366,8 @@ private: void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, float throttle_cruise, - bool climbout_mode, float climbout_pitch_min_rad); + bool climbout_mode, float climbout_pitch_min_rad, + const math::Vector<3> &ground_speed); }; @@ -426,7 +430,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode(), _global_pos(), _pos_sp_triplet(), - _sensor_combined() + _sensor_combined(), + _mTecs(), + _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; @@ -583,6 +589,9 @@ FixedwingPositionControl::parameters_update() /* Update Launch Detector Parameters */ launchDetector.updateParams(); + /* Update the mTecs */ + _mTecs.updateParams(); + return OK; } @@ -714,7 +723,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) { if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { @@ -722,7 +731,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); - float ground_speed_body = yaw_vector * ground_speed; + float ground_speed_body = yaw_vector * ground_speed_2d; /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; @@ -764,12 +773,13 @@ void FixedwingPositionControl::navigation_capabilities_publish() } bool -FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, +FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet); + math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; + calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -792,6 +802,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // else integrators should be constantly reset. if (_control_mode.flag_control_position_enabled) { + if (!_was_pos_control_mode) { + /* reset integrators */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + } + } + + _was_pos_control_mode = true; + /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -824,27 +843,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min)); + false, math::radians(_parameters.pitch_limit_min), ground_speed); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, - pos_sp_triplet.current.loiter_direction, ground_speed); + pos_sp_triplet.current.loiter_direction, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min)); + false, math::radians(_parameters.pitch_limit_min), ground_speed); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { @@ -869,7 +888,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); - _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d); /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); @@ -879,7 +898,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); } _att_sp.roll_body = _l1_control.nav_roll(); @@ -939,7 +958,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(flare_curve_alt, calculate_target_airspeed(airspeed_land), eas2tas, flare_pitch_angle_rad, math::radians(15.0f), 0.0f, throttle_max, throttle_land, - false, flare_pitch_angle_rad); + false, flare_pitch_angle_rad, ground_speed); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -971,7 +990,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(altitude_desired, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min)); + false, math::radians(_parameters.pitch_limit_min), ground_speed); } } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { @@ -998,7 +1017,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -1012,7 +1031,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f))); + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), ground_speed); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); @@ -1021,7 +1040,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min)); + false, math::radians(_parameters.pitch_limit_min), ground_speed); } } else { @@ -1055,6 +1074,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (0/* easy mode enabled */) { + _was_pos_control_mode = false; + /** EASY FLIGHT **/ if (0/* switched from another mode to easy */) { @@ -1082,16 +1103,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min)); + false, math::radians(_parameters.pitch_limit_min), ground_speed); } else if (0/* seatbelt mode enabled */) { + _was_pos_control_mode = false; + /** SEATBELT FLIGHT **/ if (0/* switched from another mode to seatbelt */) { @@ -1124,16 +1147,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed_2d); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - climb_out, math::radians(_parameters.pitch_limit_min)); + climb_out, math::radians(_parameters.pitch_limit_min), ground_speed); } else { + _was_pos_control_mode = false; + /** MANUAL FLIGHT **/ /* no flight mode applies, do not publish an attitude setpoint */ @@ -1150,9 +1175,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _tecs.get_pitch_demand(); + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1264,7 +1289,7 @@ FixedwingPositionControl::task_main() vehicle_airspeed_poll(); // vehicle_baro_poll(); - math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); + math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon); /* @@ -1329,13 +1354,25 @@ void FixedwingPositionControl::reset_landing_state() void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, float throttle_cruise, - bool climbout_mode, float climbout_pitch_min_rad) + bool climbout_mode, float climbout_pitch_min_rad, + const math::Vector<3> &ground_speed) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, - _airspeed.indicated_airspeed_m_s, eas2tas, - climbout_mode, climbout_pitch_min_rad, - throttle_min, throttle_max, throttle_cruise, - pitch_min_rad, pitch_max_rad); + if (_mTecs.getEnabled()) { + + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp); + + } else { + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, + _airspeed.indicated_airspeed_m_s, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); + } } int diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index cf419ec7e..5d752437f 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -39,4 +39,6 @@ MODULE_COMMAND = fw_pos_control_l1 SRCS = fw_pos_control_l1_main.cpp \ fw_pos_control_l1_params.c \ - landingslope.cpp + landingslope.cpp \ + mtecs/mTecs.cpp\ + mtecs/mTecs_params.c diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp new file mode 100644 index 000000000..ad108e7ee --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file mTecs.cpp + * + * @author Thomas Gubler + */ + +#include "mTecs.h" + +#include +#include + +namespace fwPosctrl { + +mTecs::mTecs() : + SuperBlock(NULL, "MT"), + /* Parameters */ + _mTecsEnabled(this, "ENABLED"), + /* control blocks */ + _controlTotalEnergy(this, "THR"), + _controlEnergyDistribution(this, "PIT", true), + _controlAltitude(this, "FPA", true), + _controlAirSpeed(this, "ACC"), + _airspeedDerivative(this, "AD"), + _throttleSp(0.0f), + _pitchSp(0.0f), + timestampLastIteration(hrt_absolute_time()), + _firstIterationAfterReset(true) +{ +} + +mTecs::~mTecs() +{ +} + +void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp) +{ + warnx("***"); + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); + warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp); +} + +void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) { + + float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); + warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp); +} + +void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp) +{ + /* time measurement */ + float deltaTSeconds = 0.0f; + if (!_firstIterationAfterReset) { + hrt_abstime timestampNow = hrt_absolute_time(); + deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f; + timestampLastIteration = timestampNow; + } + setDt(deltaTSeconds); + + /* update parameters first */ + updateParams(); + + /* calculate values (energies) */ + float flightPathAngleError = flightPathAngleSp - flightPathAngle; + float airspeedDerivative = 0.0f; + if(_airspeedDerivative.getDt() > 0.0f) { + airspeedDerivative = _airspeedDerivative.update(airspeed); + } + float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; + float airspeedSpDerivative = accelerationLongitudinalSp; + float airspeedSpDerivativeNorm = airspeedSpDerivative / CONSTANTS_ONE_G; + float airspeedSpDerivativeNormError = airspeedSpDerivativeNorm - airspeedDerivativeNorm; + + float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm; + float totalEnergyRateError = flightPathAngleError + airspeedSpDerivativeNormError; + float totalEnergyRateSp = flightPathAngleSp + airspeedSpDerivativeNorm; + float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate; + + float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm; + float energyDistributionRateError = flightPathAngleError - airspeedSpDerivativeNormError; + float energyDistributionRateSp = flightPathAngleSp - airspeedSpDerivativeNorm; + float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; + + warnx("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", + (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); + warnx("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", + (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); + + /** update control blocks **/ + /* update total energy rate control block */ + _throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError); + + /* update energy distribution rate control block */ + _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError); + + warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", + (double)_throttleSp, (double)_pitchSp, + (double)flightPathAngleSp, (double)flightPathAngle, + (double)accelerationLongitudinalSp, (double)airspeedDerivative); + + + /* clean up */ + _firstIterationAfterReset = false; + +} + +void mTecs::resetIntegrators() +{ + _controlTotalEnergy.getIntegral().setY(0.0f); + _controlEnergyDistribution.getIntegral().setY(0.0f); + timestampLastIteration = hrt_absolute_time(); + _firstIterationAfterReset = true; +} + +} /* namespace fwPosctrl */ + diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h new file mode 100644 index 000000000..5877a8312 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file mTecs.h + * + * @author Thomas Gubler + */ + + +#ifndef MTECS_H_ +#define MTECS_H_ + +#include "mTecs_blocks.h" + +#include +#include + +namespace fwPosctrl +{ + +/* Main class of the mTecs */ +class mTecs : public control::SuperBlock +{ +public: + mTecs(); + virtual ~mTecs(); + + /* + * Control in altitude setpoint and speed mode + */ + void updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp); + + /* + * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode + */ + void updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp); + + /* + * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) + */ + void updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp); + + /* + * Reset all integrators + */ + void resetIntegrators(); + + + /* Accessors */ + bool getEnabled() {return _mTecsEnabled.get() > 0;} + float getThrottleSetpoint() {return _throttleSp;} + float getPitchSetpoint() {return _pitchSp;} + +protected: + /* parameters */ + control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ + + /* control blocks */ + BlockFFPILimited _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ + BlockFFPILimited _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ + BlockPLimited _controlAltitude; /**< P controller for altitude: output is the flight path angle setpoint */ + BlockPLimited _controlAirSpeed; /**< P controller for airspeed: output is acceleration setpoint */ + + /* Other calculation Blocks */ + control::BlockDerivative _airspeedDerivative; + + /* Output setpoints */ + float _throttleSp; /**< Throttle Setpoint from 0 to 1 */ + float _pitchSp; /**< Pitch Setpoint from -pi to pi */ + + /* Time measurements */ + hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ + + bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + +}; + +} /* namespace fwPosctrl */ + +#endif /* MTECS_H_ */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h new file mode 100644 index 000000000..7dd5c7c2e --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -0,0 +1,198 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file mTecs_blocks.h + * + * Custom blocks for the mTecs + * + * @author Thomas Gubler + */ + +#pragma once + +#include +#include + +namespace fwPosctrl +{ + +using namespace control; + +/* Integrator without limit */ +class BlockIntegralNoLimit: public SuperBlock +{ +public: +// methods + BlockIntegralNoLimit(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _y(0) {}; + virtual ~BlockIntegralNoLimit() {}; + float update(float input) { + setY(getY() + input * getDt()); + return getY(); + }; +// accessors + float getY() {return _y;} + void setY(float y) {_y = y;} +protected: +// attributes + float _y; /**< previous output */ +}; + +/* An block which can be used to limit the output */ +class BlockOutputLimiter: public SuperBlock +{ +public: +// methods + BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _isAngularLimit(isAngularLimit), + _min(this, "MIN"), + _max(this, "MAX") + {}; + virtual ~BlockOutputLimiter() {}; + bool limit(float& value, float& difference) { + float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); + float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); + char name[blockNameLengthMax]; + getName(name, blockNameLengthMax); + warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", + name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); + if (value < minimum) { + difference = value - minimum; + value = minimum; + return true; + } else if (value > maximum) { + difference = value - maximum; + value = maximum; + return true; + } + return false; + } +//accessor: + bool isAngularLimit() {return _isAngularLimit ;} + float getMin() { return _min.get(); } + float getMax() { return _max.get(); } +protected: +//attributes + bool _isAngularLimit; + control::BlockParamFloat _min; + control::BlockParamFloat _max; +}; + +typedef + +/* A combination of feed forward, P and I gain using the output limiter*/ +class BlockFFPILimited: public SuperBlock +{ +public: +// methods + BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _integral(this, "I"), + _kFF(this, "FF"), + _kP(this, "P"), + _kI(this, "I"), + _offset(this, "OFF"), + _outputLimiter(this, "", isAngularLimit) + {}; + virtual ~BlockFFPILimited() {}; + float update(float inputValue, float inputError) { + float difference = 0.0f; + float integralYPrevious = _integral.getY(); + float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); + char name[blockNameLengthMax]; + getName(name, blockNameLengthMax); + warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, + (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); + if(!getOutputLimiter().limit(output, difference) && + (((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) || + ((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) { + getIntegral().setY(integralYPrevious); + } + warnx("%s output limited %.2f", + name,(double)output); + return output; + } +// accessors + BlockIntegralNoLimit &getIntegral() { return _integral; } + float getKFF() { return _kFF.get(); } + float getKP() { return _kP.get(); } + float getKI() { return _kI.get(); } + float getOffset() { return _offset.get(); } + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; +private: + BlockIntegralNoLimit _integral; + BlockParamFloat _kFF; + BlockParamFloat _kP; + BlockParamFloat _kI; + BlockParamFloat _offset; + BlockOutputLimiter _outputLimiter; +}; + +/* A combination of P gain and output limiter */ +class BlockPLimited: public SuperBlock +{ +public: +// methods + BlockPLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _kP(this, "P"), + _outputLimiter(this, "", isAngularLimit) + {}; + virtual ~BlockPLimited() {}; + float update(float input) { + float difference = 0.0f; + float output = getKP() * input; + char name[blockNameLengthMax]; + getName(name, blockNameLengthMax); + warnx("%s output %.2f _kP.get() %.2f, input", + name,(double)output, (double)_kP.get(), (double)input); + getOutputLimiter().limit(output, difference); + warnx("%s output limited %.2f", + name,(double)output); + return output; + } +// accessors + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; + float getKP() { return _kP.get(); } +private: + control::BlockParamFloat _kP; + BlockOutputLimiter _outputLimiter; +}; + +} + diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c new file mode 100644 index 000000000..76ed95303 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -0,0 +1,231 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file mTecs_params.c + * + * @author Thomas Gubler + */ + +#include +#include + +/* + * Controller parameters, accessible via MAVLink + */ + +/** + * mTECS enabled + * + * Set to 1 to enable mTECS + * + * @min 0 + * @max 1 + * @group mTECS + */ +PARAM_DEFINE_INT32(MT_ENABLED, 0); + +/** + * Total Energy Rate Control FF + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_FF, 1.0f); + +/** + * Total Energy Rate Control P + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f); + +/** + * Total Energy Rate Control I + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_I, 0.0f); + +/** + * Total Energy Rate Control OFF (Cruise throttle) + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f); + +/** + * Energy Distribution Rate Control FF + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_FF, 1.0f); + +/** + * Energy Distribution Rate Control P + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_P, 0.1f); + +/** + * Energy Distribution Rate Control I + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_I, 0.0f); + + +/** + * Total Energy Distribution OFF (Cruise pitch sp) + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_OFF, 0.0f); + +/** + * Minimal Throttle Setpoint + * + * @min 0.0 + * @max 1.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_MIN, 0.0f); + +/** + * Maximal Throttle Setpoint + * + * @min 0.0 + * @max 1.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f); + +/** + * Minimal Pitch Setpoint in Degrees + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); + +/** + * Maximal Pitch Setpoint in Degrees + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_MAX, 45.0f); + +/** + * P gain for the altitude control + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_P, 0.1f); + +/** + * Minimal flight path angle setpoint + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_MIN, -30.0f); + +/** + * Maximal flight path angle setpoint + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); + + +/** + * P gain for the airspeed control + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_P, 0.1f); + +/** + * Minimal acceleration (air) + * + * @unit m/s^2 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_MIN, 0.0f); + +/** + * Maximal acceleration (air) + * +* @unit m/s^2 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); + +/** + * Airspeed derivative lowpass + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); -- cgit v1.2.3 From b61e6f2706fed68257c909bdd8a84feda5121344 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Mar 2014 21:23:48 +0100 Subject: mtecs: don't flow the stdout too much --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 35 ++++++++++++++++++--------- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 1 + 2 files changed, 24 insertions(+), 12 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index ad108e7ee..fa9a6d947 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -59,7 +59,8 @@ mTecs::mTecs() : _throttleSp(0.0f), _pitchSp(0.0f), timestampLastIteration(hrt_absolute_time()), - _firstIterationAfterReset(true) + _firstIterationAfterReset(true), + _counter(0) { } @@ -69,16 +70,21 @@ mTecs::~mTecs() void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp) { - warnx("***"); + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); - warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + if (_counter % 10 == 0) { + warnx("***"); + warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + } updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp); } void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) { float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); - warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + if (_counter % 10 == 0) { + warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + } updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp); } @@ -117,10 +123,12 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh float energyDistributionRateSp = flightPathAngleSp - airspeedSpDerivativeNorm; float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; - warnx("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", - (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); - warnx("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", - (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); + if (_counter % 10 == 0) { + warnx("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", + (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); + warnx("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", + (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); + } /** update control blocks **/ /* update total energy rate control block */ @@ -129,15 +137,18 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh /* update energy distribution rate control block */ _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError); - warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", - (double)_throttleSp, (double)_pitchSp, - (double)flightPathAngleSp, (double)flightPathAngle, - (double)accelerationLongitudinalSp, (double)airspeedDerivative); + if (_counter % 10 == 0) { + warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", + (double)_throttleSp, (double)_pitchSp, + (double)flightPathAngleSp, (double)flightPathAngle, + (double)accelerationLongitudinalSp, (double)airspeedDerivative); + } /* clean up */ _firstIterationAfterReset = false; + _counter++; } void mTecs::resetIntegrators() diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 5877a8312..32d9879db 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -106,6 +106,7 @@ protected: bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + int _counter; }; } /* namespace fwPosctrl */ -- cgit v1.2.3 From c81aefc756c6587e306986312370cf9d22a5f534 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Mar 2014 21:24:20 +0100 Subject: mtecs: commented out warnx --- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index 7dd5c7c2e..aff01479d 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -89,8 +89,8 @@ public: float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); char name[blockNameLengthMax]; getName(name, blockNameLengthMax); - warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", - name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); +// warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", +// name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); if (value < minimum) { difference = value - minimum; value = minimum; @@ -136,15 +136,15 @@ public: float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); char name[blockNameLengthMax]; getName(name, blockNameLengthMax); - warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, - (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); +// warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, +// (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); if(!getOutputLimiter().limit(output, difference) && (((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) || ((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) { getIntegral().setY(integralYPrevious); } - warnx("%s output limited %.2f", - name,(double)output); +// warnx("%s output limited %.2f", +// name,(double)output); return output; } // accessors @@ -179,11 +179,11 @@ public: float output = getKP() * input; char name[blockNameLengthMax]; getName(name, blockNameLengthMax); - warnx("%s output %.2f _kP.get() %.2f, input", - name,(double)output, (double)_kP.get(), (double)input); +// warnx("%s output %.2f _kP.get() %.2f, input", +// name,(double)output, (double)_kP.get(), (double)input); getOutputLimiter().limit(output, difference); - warnx("%s output limited %.2f", - name,(double)output); +// warnx("%s output limited %.2f", +// name,(double)output); return output; } // accessors -- cgit v1.2.3 From f949395edf873b423cae3d62a95cd0800a2d344f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Mar 2014 22:25:16 +0100 Subject: mtecs: correct a very wrong default param value --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 76ed95303..705b2cd59 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -213,7 +213,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_P, 0.1f); * @unit m/s^2 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_ACC_MIN, 0.0f); +PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); /** * Maximal acceleration (air) -- cgit v1.2.3 From d8eeec6cdbff505ac7f50877cc552b33ccf8837c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Mar 2014 23:03:06 +0100 Subject: mtecs: comment out debug code --- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index aff01479d..c15d90cdb 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -87,8 +87,8 @@ public: bool limit(float& value, float& difference) { float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); - char name[blockNameLengthMax]; - getName(name, blockNameLengthMax); +// char name[blockNameLengthMax]; +// getName(name, blockNameLengthMax); // warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", // name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); if (value < minimum) { @@ -134,8 +134,8 @@ public: float difference = 0.0f; float integralYPrevious = _integral.getY(); float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); - char name[blockNameLengthMax]; - getName(name, blockNameLengthMax); +// char name[blockNameLengthMax]; +// getName(name, blockNameLengthMax); // warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, // (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); if(!getOutputLimiter().limit(output, difference) && @@ -177,8 +177,8 @@ public: float update(float input) { float difference = 0.0f; float output = getKP() * input; - char name[blockNameLengthMax]; - getName(name, blockNameLengthMax); +// char name[blockNameLengthMax]; +// getName(name, blockNameLengthMax); // warnx("%s output %.2f _kP.get() %.2f, input", // name,(double)output, (double)_kP.get(), (double)input); getOutputLimiter().limit(output, difference); -- cgit v1.2.3 From 4824484497cf0d85d08fe9c9727aa5edc4446e67 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 27 Mar 2014 19:03:57 +0100 Subject: mtecs: add FPA D gain --- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 2 +- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 48 ++++++++++++++-------- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 17 ++++++++ 3 files changed, 50 insertions(+), 17 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 32d9879db..502a8a8b0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -91,7 +91,7 @@ protected: /* control blocks */ BlockFFPILimited _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ BlockFFPILimited _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ - BlockPLimited _controlAltitude; /**< P controller for altitude: output is the flight path angle setpoint */ + BlockPDLimited _controlAltitude; /**< P controller for altitude: output is the flight path angle setpoint */ BlockPLimited _controlAirSpeed; /**< P controller for airspeed: output is acceleration setpoint */ /* Other calculation Blocks */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index c15d90cdb..a357b24e2 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -87,10 +87,6 @@ public: bool limit(float& value, float& difference) { float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); -// char name[blockNameLengthMax]; -// getName(name, blockNameLengthMax); -// warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", -// name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); if (value < minimum) { difference = value - minimum; value = minimum; @@ -134,17 +130,11 @@ public: float difference = 0.0f; float integralYPrevious = _integral.getY(); float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); -// char name[blockNameLengthMax]; -// getName(name, blockNameLengthMax); -// warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, -// (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); if(!getOutputLimiter().limit(output, difference) && (((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) || ((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) { getIntegral().setY(integralYPrevious); } -// warnx("%s output limited %.2f", -// name,(double)output); return output; } // accessors @@ -177,13 +167,7 @@ public: float update(float input) { float difference = 0.0f; float output = getKP() * input; -// char name[blockNameLengthMax]; -// getName(name, blockNameLengthMax); -// warnx("%s output %.2f _kP.get() %.2f, input", -// name,(double)output, (double)_kP.get(), (double)input); getOutputLimiter().limit(output, difference); -// warnx("%s output limited %.2f", -// name,(double)output); return output; } // accessors @@ -194,5 +178,37 @@ private: BlockOutputLimiter _outputLimiter; }; +/* A combination of P, D gains and output limiter */ +class BlockPDLimited: public SuperBlock +{ +public: +// methods + BlockPDLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _kP(this, "P"), + _kD(this, "D"), + _derivative(this, "D"), + _outputLimiter(this, "", isAngularLimit) + {}; + virtual ~BlockPDLimited() {}; + float update(float input) { + float difference = 0.0f; + float output = getKP() * input + getKD() * getDerivative().update(input); + getOutputLimiter().limit(output, difference); + + return output; + } +// accessors + float getKP() { return _kP.get(); } + float getKD() { return _kD.get(); } + BlockDerivative &getDerivative() { return _derivative; } + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; +private: + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; + BlockDerivative _derivative; + BlockOutputLimiter _outputLimiter; +}; + } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 705b2cd59..f8db70304 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -177,6 +177,23 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 45.0f); */ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.1f); +/** + * D gain for the altitude control + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f); + +/** + * Lowpass for FPA error derivative (see MT_FPA_D) + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f); + + /** * Minimal flight path angle setpoint * -- cgit v1.2.3 From 0d526bddca9210bed75cfd26458a48ac87453c9e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 27 Mar 2014 21:47:34 +0100 Subject: fw_pos_control: whitespace in module.mk --- src/modules/fw_pos_control_l1/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index 5d752437f..c887223f4 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -40,5 +40,5 @@ MODULE_COMMAND = fw_pos_control_l1 SRCS = fw_pos_control_l1_main.cpp \ fw_pos_control_l1_params.c \ landingslope.cpp \ - mtecs/mTecs.cpp\ + mtecs/mTecs.cpp \ mtecs/mTecs_params.c -- cgit v1.2.3 From d102afba8bdb526b85dce6dc47034ddd170d7240 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 27 Mar 2014 22:12:01 +0100 Subject: mtecs: make sure dt is calculated before any control calculations --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 31 +++++++++++++++++++++------ src/modules/fw_pos_control_l1/mtecs/mTecs.h | 8 ++++++- 2 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index fa9a6d947..3118ea5d1 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -60,6 +60,7 @@ mTecs::mTecs() : _pitchSp(0.0f), timestampLastIteration(hrt_absolute_time()), _firstIterationAfterReset(true), + dtCalculated(false), _counter(0) { } @@ -71,6 +72,9 @@ mTecs::~mTecs() void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp) { + /* time measurement */ + updateTimeMeasurement(); + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); if (_counter % 10 == 0) { warnx("***"); @@ -81,6 +85,9 @@ void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alt void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) { + /* time measurement */ + updateTimeMeasurement(); + float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); if (_counter % 10 == 0) { warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); @@ -91,13 +98,7 @@ void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAn void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp) { /* time measurement */ - float deltaTSeconds = 0.0f; - if (!_firstIterationAfterReset) { - hrt_abstime timestampNow = hrt_absolute_time(); - deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f; - timestampLastIteration = timestampNow; - } - setDt(deltaTSeconds); + updateTimeMeasurement(); /* update parameters first */ updateParams(); @@ -147,6 +148,7 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh /* clean up */ _firstIterationAfterReset = false; + dtCalculated = false; _counter++; } @@ -159,5 +161,20 @@ void mTecs::resetIntegrators() _firstIterationAfterReset = true; } +void mTecs::updateTimeMeasurement() +{ + if (!dtCalculated) { + float deltaTSeconds = 0.0f; + if (!_firstIterationAfterReset) { + hrt_abstime timestampNow = hrt_absolute_time(); + deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f; + timestampLastIteration = timestampNow; + } + setDt(deltaTSeconds); + + dtCalculated = true; + } +} + } /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 502a8a8b0..62c4d7014 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -104,9 +104,15 @@ protected: /* Time measurements */ hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ - bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + bool dtCalculated; /**< True if dt has been calculated in this iteration */ int _counter; + + /* + * Measure and update the time step dt if this was not already done in the current iteration + */ + void updateTimeMeasurement(); }; } /* namespace fwPosctrl */ -- cgit v1.2.3 From d3ca12f136c9a890f7289d69bd783840dc86cfba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 27 Mar 2014 22:34:23 +0100 Subject: mtecs: BlockPDLimited: make sure dt > 0 --- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index a357b24e2..6e90a82ba 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -193,7 +193,7 @@ public: virtual ~BlockPDLimited() {}; float update(float input) { float difference = 0.0f; - float output = getKP() * input + getKD() * getDerivative().update(input); + float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f); getOutputLimiter().limit(output, difference); return output; -- cgit v1.2.3 From 3e9dfcb6f7bbda7653e6d8873b6273f2e9299d73 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 27 Mar 2014 22:57:29 +0100 Subject: mtecs: first rough version of takeoff mode --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 +++++- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 25 ++++++++---- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 19 ++++++--- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 46 +++++++++++++++------- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 39 ++++++++++++++++++ 5 files changed, 112 insertions(+), 28 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 1e8447aed..44afa43d8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -361,7 +361,7 @@ private: void reset_landing_state(); /* - * Call TECS + * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter) */ void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, float pitch_min_rad, float pitch_max_rad, @@ -1364,7 +1364,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ if (ground_speed_length > FLT_EPSILON) { flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } - _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp); + + if (!climbout_mode) { + /* Normal operation */ + _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_NORMAL); + } else { + /* Climbout/Takeoff mode requested */ + _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_TAKEOFF); + } } else { _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 3118ea5d1..33bfd4962 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -58,6 +58,8 @@ mTecs::mTecs() : _airspeedDerivative(this, "AD"), _throttleSp(0.0f), _pitchSp(0.0f), + BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"), + BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), timestampLastIteration(hrt_absolute_time()), _firstIterationAfterReset(true), dtCalculated(false), @@ -69,7 +71,7 @@ mTecs::~mTecs() { } -void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp) +void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode) { /* time measurement */ @@ -80,10 +82,10 @@ void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alt warnx("***"); warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); } - updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp); + updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode); } -void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) { +void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode) { /* time measurement */ updateTimeMeasurement(); @@ -92,10 +94,10 @@ void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAn if (_counter % 10 == 0) { warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); } - updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp); + updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode); } -void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp) +void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode) { /* time measurement */ updateTimeMeasurement(); @@ -131,12 +133,21 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); } + /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ + BlockOutputLimiter *outputLimiterThrottle = NULL; + BlockOutputLimiter *outputLimiterPitch = NULL; + if (mode == TECS_MODE_TAKEOFF) { + outputLimiterThrottle = &BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector + outputLimiterPitch = &BlockOutputLimiterTakeoffPitch; + } + /** update control blocks **/ /* update total energy rate control block */ - _throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError); + _throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle); /* update energy distribution rate control block */ - _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError); + _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError, outputLimiterPitch); + if (_counter % 10 == 0) { warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 62c4d7014..6c30a458a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -58,20 +58,25 @@ public: mTecs(); virtual ~mTecs(); + typedef enum { + TECS_MODE_NORMAL, + TECS_MODE_TAKEOFF + } tecs_mode; + /* * Control in altitude setpoint and speed mode */ - void updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp); + void updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode */ - void updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp); + void updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) */ - void updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp); + void updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode); /* * Reset all integrators @@ -89,8 +94,8 @@ protected: control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ /* control blocks */ - BlockFFPILimited _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ - BlockFFPILimited _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ + BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ + BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ BlockPDLimited _controlAltitude; /**< P controller for altitude: output is the flight path angle setpoint */ BlockPLimited _controlAirSpeed; /**< P controller for airspeed: output is acceleration setpoint */ @@ -101,6 +106,10 @@ protected: float _throttleSp; /**< Throttle Setpoint from 0 to 1 */ float _pitchSp; /**< Pitch Setpoint from -pi to pi */ + /* Output Limits in special modes */ + BlockOutputLimiter BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + /* Time measurements */ hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index 6e90a82ba..f3dc9bcb2 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -111,46 +111,64 @@ protected: typedef -/* A combination of feed forward, P and I gain using the output limiter*/ +/* A combination of feed forward, P and I gain using the output limiter*/ class BlockFFPILimited: public SuperBlock { public: // methods BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : SuperBlock(parent, name), + _outputLimiter(this, "", isAngularLimit), _integral(this, "I"), _kFF(this, "FF"), _kP(this, "P"), _kI(this, "I"), - _offset(this, "OFF"), - _outputLimiter(this, "", isAngularLimit) + _offset(this, "OFF") {}; virtual ~BlockFFPILimited() {}; - float update(float inputValue, float inputError) { + float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); } +// accessors + BlockIntegralNoLimit &getIntegral() { return _integral; } + float getKFF() { return _kFF.get(); } + float getKP() { return _kP.get(); } + float getKI() { return _kI.get(); } + float getOffset() { return _offset.get(); } + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; +protected: + BlockOutputLimiter _outputLimiter; + + float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);} + float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) { float difference = 0.0f; float integralYPrevious = _integral.getY(); - float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); - if(!getOutputLimiter().limit(output, difference) && + float output = calcUnlimitedOutput(inputValue, inputError); + if(!outputLimiter.limit(output, difference) && (((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) || ((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) { getIntegral().setY(integralYPrevious); } return output; } -// accessors - BlockIntegralNoLimit &getIntegral() { return _integral; } - float getKFF() { return _kFF.get(); } - float getKP() { return _kP.get(); } - float getKI() { return _kI.get(); } - float getOffset() { return _offset.get(); } - BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; private: BlockIntegralNoLimit _integral; BlockParamFloat _kFF; BlockParamFloat _kP; BlockParamFloat _kI; BlockParamFloat _offset; - BlockOutputLimiter _outputLimiter; +}; + +/* A combination of feed forward, P and I gain using the output limiter with the option to provide a special output limiter (for example for takeoff)*/ +class BlockFFPILimitedCustom: public BlockFFPILimited +{ +public: +// methods + BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + BlockFFPILimited(parent, name, isAngularLimit) + {}; + virtual ~BlockFFPILimitedCustom() {}; + float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) { + return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter); + } }; /* A combination of P gain and output limiter */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index f8db70304..bbd957273 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -246,3 +246,42 @@ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); * @group mTECS */ PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); + + +/** + * Minimal throttle during takeoff + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f); + +/** + * Maximal throttle during takeoff + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f); + +/** + * Minimal pitch during takeoff + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f); + +/** + * Maximal pitch during takeoff + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f); -- cgit v1.2.3 From 183a0cdb22fd824d87912ea3d2c2470f0d28ed39 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 4 Apr 2014 12:33:02 +0400 Subject: MC: default MC_YAWRATE_I changed for all setups, navigator: increase yaw acceptance to 0.2rad ~ 11deg --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 2 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 2 +- ROMFS/px4fmu_common/init.d/4008_ardrone | 2 +- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 2 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 2 +- src/modules/navigator/navigator_main.cpp | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index fe85f7d35..c4be16943 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -20,7 +20,7 @@ then param set MC_PITCHRATE_D 0.0025 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.28 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index f11aa704e..26bea688a 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -20,7 +20,7 @@ then param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 0.5 param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 param set BAT_V_SCALING 0.00989 diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 14786f210..0c3cd09dd 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -24,7 +24,7 @@ then param set MC_PITCHRATE_D 0.0 param set MC_YAW_P 1.0 param set MC_YAWRATE_P 0.15 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.05 param set MC_YAWRATE_D 0.0 param set MC_YAW_FF 0.15 param set BAT_V_SCALING 0.00838095238 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index cd4480c3e..e6e2e19dc 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -19,7 +19,7 @@ then param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index ac2ecc70a..3465b59cf 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -20,7 +20,7 @@ then param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 4db62607a..e96c5b493 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -14,7 +14,7 @@ then param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 param set MC_YAW_FF 0.5 diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1b..5a94b6671 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1539,7 +1539,7 @@ Navigator::check_mission_item_reached() /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); - if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } -- cgit v1.2.3 From 45cef9eed4a9f1c6761f41a187672cc4ba82cd39 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 6 Apr 2014 11:11:58 +0200 Subject: mtecs: better (but not final) default parameters for mtecs --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index f8db70304..194dfa150 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -55,7 +55,7 @@ * @max 1 * @group mTECS */ -PARAM_DEFINE_INT32(MT_ENABLED, 0); +PARAM_DEFINE_INT32(MT_ENABLED, 1); /** * Total Energy Rate Control FF @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(MT_ENABLED, 0); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_THR_FF, 1.0f); +PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f); /** * Total Energy Rate Control P @@ -73,7 +73,7 @@ PARAM_DEFINE_FLOAT(MT_THR_FF, 1.0f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f); +PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f); /** * Total Energy Rate Control I @@ -82,7 +82,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_THR_I, 0.0f); +PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f); /** * Total Energy Rate Control OFF (Cruise throttle) @@ -100,7 +100,7 @@ PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_PIT_FF, 1.0f); +PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.1f); /** * Energy Distribution Rate Control P @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_FF, 1.0f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_PIT_P, 0.1f); +PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f); /** * Energy Distribution Rate Control I @@ -118,7 +118,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_P, 0.1f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_PIT_I, 0.0f); +PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f); /** @@ -166,7 +166,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); * @unit deg * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_PIT_MAX, 45.0f); +PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); /** * P gain for the altitude control @@ -175,7 +175,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 45.0f); * @max 10.0f * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_FPA_P, 0.1f); +PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f); /** * D gain for the altitude control @@ -202,7 +202,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f); * @unit deg * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_FPA_MIN, -30.0f); +PARAM_DEFINE_FLOAT(MT_FPA_MIN, -10.0f); /** * Maximal flight path angle setpoint @@ -222,7 +222,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); * @max 10.0f * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_ACC_P, 0.1f); +PARAM_DEFINE_FLOAT(MT_ACC_P, 1.5f); /** * Minimal acceleration (air) -- cgit v1.2.3 From 8fcdff15a480399541b9ecccf8dc8260d437ec7f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 6 Apr 2014 14:58:31 +0200 Subject: mtecs: add comment about setting the standard limits --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 33bfd4962..63e60995e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -134,8 +134,8 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh } /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ - BlockOutputLimiter *outputLimiterThrottle = NULL; - BlockOutputLimiter *outputLimiterPitch = NULL; + BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits + BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits if (mode == TECS_MODE_TAKEOFF) { outputLimiterThrottle = &BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector outputLimiterPitch = &BlockOutputLimiterTakeoffPitch; -- cgit v1.2.3 From 662a7403b2ef00018d6c1b38265ec0ba4a9ae6bf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 22:36:28 +0400 Subject: mavlink: REQUEST_DATA_STREAM hadling implemented --- src/modules/mavlink/mavlink_main.h | 3 +- src/modules/mavlink/mavlink_messages.cpp | 112 ++++++++++++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.cpp | 21 ++++++ src/modules/mavlink/mavlink_receiver.h | 1 + src/modules/mavlink/mavlink_stream.h | 1 + 5 files changed, 135 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5a118a0ad..427b9ad35 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -194,6 +194,8 @@ public: mavlink_channel_t get_channel(); + void configure_stream_threadsafe(const char *stream_name, const float rate); + bool _task_should_exit; /**< if true, mavlink task should exit */ protected: @@ -313,7 +315,6 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int configure_stream(const char *stream_name, const float rate); - void configure_stream_threadsafe(const char *stream_name, const float rate); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c89031fcc..5b285dc9b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -193,6 +193,11 @@ public: return "HEARTBEAT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HEARTBEAT; + } + MavlinkStream *new_instance() { return new MavlinkStreamHeartbeat(); @@ -244,6 +249,11 @@ public: return "SYS_STATUS"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_SYS_STATUS; + } + MavlinkStream *new_instance() { return new MavlinkStreamSysStatus(); @@ -293,6 +303,11 @@ public: return "HIGHRES_IMU"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIGHRES_IMU; + } + MavlinkStream *new_instance() { return new MavlinkStreamHighresIMU(); @@ -364,6 +379,11 @@ public: return "ATTITUDE"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE; + } + MavlinkStream *new_instance() { return new MavlinkStreamAttitude(); @@ -400,6 +420,11 @@ public: return "ATTITUDE_QUATERNION"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; + } + MavlinkStream *new_instance() { return new MavlinkStreamAttitudeQuaternion(); @@ -441,6 +466,11 @@ public: return "VFR_HUD"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_VFR_HUD; + } + MavlinkStream *new_instance() { return new MavlinkStreamVFRHUD(); @@ -514,6 +544,11 @@ public: return "GPS_RAW_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_RAW_INT; + } + MavlinkStream *new_instance() { return new MavlinkStreamGPSRawInt(); @@ -557,6 +592,11 @@ public: return "GLOBAL_POSITION_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + } + MavlinkStream *new_instance() { return new MavlinkStreamGlobalPositionInt(); @@ -608,6 +648,11 @@ public: return "LOCAL_POSITION_NED"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED; + } + MavlinkStream *new_instance() { return new MavlinkStreamLocalPositionNED(); @@ -648,6 +693,11 @@ public: return "GPS_GLOBAL_ORIGIN"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; + } + MavlinkStream *new_instance() { return new MavlinkStreamGPSGlobalOrigin(); @@ -689,6 +739,11 @@ public: sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); } + uint8_t get_id() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + } + const char *get_name() { return _name; @@ -747,6 +802,11 @@ public: return "HIL_CONTROLS"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIL_CONTROLS; + } + MavlinkStream *new_instance() { return new MavlinkStreamHILControls(); @@ -874,6 +934,11 @@ public: return "GLOBAL_POSITION_SETPOINT_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + } + MavlinkStream *new_instance() { return new MavlinkStreamGlobalPositionSetpointInt(); @@ -912,6 +977,11 @@ public: return "LOCAL_POSITION_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + } + MavlinkStream *new_instance() { return new MavlinkStreamLocalPositionSetpoint(); @@ -950,6 +1020,11 @@ public: return "ROLL_PITCH_YAW_THRUST_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + } + MavlinkStream *new_instance() { return new MavlinkStreamRollPitchYawThrustSetpoint(); @@ -988,6 +1063,11 @@ public: return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; + } + MavlinkStream *new_instance() { return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); @@ -1026,6 +1106,11 @@ public: return "RC_CHANNELS_RAW"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_RC_CHANNELS_RAW; + } + MavlinkStream *new_instance() { return new MavlinkStreamRCChannelsRaw(); @@ -1075,6 +1160,11 @@ public: return "MANUAL_CONTROL"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_MANUAL_CONTROL; + } + MavlinkStream *new_instance() { return new MavlinkStreamManualControl(); @@ -1114,6 +1204,11 @@ public: return "OPTICAL_FLOW"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_OPTICAL_FLOW; + } + MavlinkStream *new_instance() { return new MavlinkStreamOpticalFlow(); @@ -1152,6 +1247,11 @@ public: return "ATTITUDE_CONTROLS"; } + uint8_t get_id() + { + return 0; + } + MavlinkStream *new_instance() { return new MavlinkStreamAttitudeControls(); @@ -1200,6 +1300,11 @@ public: return "NAMED_VALUE_FLOAT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + } + MavlinkStream *new_instance() { return new MavlinkStreamNamedValueFloat(); @@ -1238,6 +1343,11 @@ public: return "CAMERA_CAPTURE"; } + uint8_t get_id() + { + return 0; + } + MavlinkStream *new_instance() { return new MavlinkStreamCameraCapture(); @@ -1252,8 +1362,6 @@ protected: { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); - - } void send(const hrt_abstime t) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ec40ee0a..cf8b23bc3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -152,6 +152,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_manual_control(msg); break; + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + handle_message_request_data_stream(msg); + break; + default: break; } @@ -457,6 +461,23 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) +{ + mavlink_request_data_stream_t req; + mavlink_msg_request_data_stream_decode(msg, &req); + + if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) { + float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f; + + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + if (req.req_stream_id == 0 || req.req_stream_id == streams_list[i]->get_id()) { + _mavlink->configure_stream_threadsafe(streams_list[i]->get_name(), rate); + } + } + } +} + void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8ccb2a035..eab8f071d 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,6 +112,7 @@ private: void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_request_data_stream(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 135e1bce0..aa8ca450b 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -70,6 +70,7 @@ public: virtual MavlinkStream *new_instance() = 0; virtual void subscribe(Mavlink *mavlink) = 0; virtual const char *get_name() = 0; + virtual uint8_t get_id() = 0; }; -- cgit v1.2.3 From 2ac0e1fc547683a092eef85678652c6a538a870e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 31 Mar 2014 10:10:50 +0400 Subject: rcS: removed unnecessary sleeps, minor code style fixes --- ROMFS/px4fmu_common/init.d/rcS | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 503dbb83e..0cb6d281a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -294,6 +294,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi + if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then echo "[init] Use FMU as primary output" @@ -317,6 +318,7 @@ then fi fi fi + if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" @@ -338,7 +340,8 @@ then tone_alarm $TUNE_OUT_ERROR fi - fi + fi + if [ $OUTPUT_MODE == hil ] then echo "[init] Use HIL as primary output" @@ -396,34 +399,29 @@ then # # MAVLink # - if [ $MAVLINK_FLAGS == default ] then if [ $HIL == yes ] then sleep 1 set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0" - usleep 5000 else # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" - usleep 5000 # Exit from nsh to free port for mavlink set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 set MAVLINK_FLAGS "-r 1000" - usleep 5000 fi fi fi mavlink start $MAVLINK_FLAGS - usleep 5000 # # Start the datamanager -- cgit v1.2.3 From 3fa82675e79013fedf9a787ca21e06e5bd15e5ca Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 3 Apr 2014 21:13:03 +0200 Subject: commander: don't beep if message is not understood --- src/modules/commander/commander.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d4567c4f1..27d5e4260 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -591,11 +591,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - - if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { - /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, result); - } + /* silently ignore unsupported commands, maybe they are passed on over mavlink */ +// if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { +// /* already warned about unsupported commands in "default" case */ +// answer_command(*cmd, result); +// } /* send any requested ACKs */ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -- cgit v1.2.3 From 15defeb0f1e6ce396562685316a8734fcd867081 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 3 Apr 2014 21:15:47 +0200 Subject: mavlink: implemented multicasting between mavlink instances (two options: forwarding: forward received messages from self to other mavlink instances, passing: send out messages received from other mavlink intances over serial --- src/modules/mavlink/mavlink_main.cpp | 197 ++++++++++++++++++++++++++++++- src/modules/mavlink/mavlink_main.h | 34 +++++- src/modules/mavlink/mavlink_messages.cpp | 6 +- src/modules/mavlink/mavlink_receiver.cpp | 5 + 4 files changed, 236 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 18df577fe..c5055939e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -207,10 +207,15 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _uart_fd(-1), _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _message_buffer({}), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -261,7 +266,6 @@ Mavlink::Mavlink() : errx(1, "instance ID is out of range"); break; } - } Mavlink::~Mavlink() @@ -394,6 +398,20 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) return false; } +void +Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) +{ + Mavlink *inst = ::_mavlink_instances; + + while (inst != nullptr) { + /* don't broadcast to itself */ + if (inst != self) { + inst->pass_message(msg); + } + inst = inst->next; + } +} + int Mavlink::get_uart_fd(unsigned index) { @@ -1616,6 +1634,125 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) } } +int +Mavlink::message_buffer_init(int size) +{ + _message_buffer.size = size; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + _message_buffer.data = (char*)malloc(_message_buffer.size); + return (_message_buffer.data == 0) ? ERROR : OK; +} + +void +Mavlink::message_buffer_destroy() +{ + _message_buffer.size = 0; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + free(_message_buffer.data); +} + +int +Mavlink::message_buffer_count() +{ + int n = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (n < 0) { + n += _message_buffer.size; + } + + return n; +} + +int +Mavlink::message_buffer_is_empty() +{ + return _message_buffer.read_ptr == _message_buffer.write_ptr; +} + + +bool +Mavlink::message_buffer_write(void *ptr, int size) +{ + // bytes available to write + int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; + + if (available < 0) { + available += _message_buffer.size; + } + + if (size > available) { + // buffer overflow + return false; + } + + char *c = (char *) ptr; + int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer + + if (n < size) { + // message goes over end of the buffer + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); + _message_buffer.write_ptr = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + int p = size - n; // number of bytes to write + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); + _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; + return true; +} + +int +Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) +{ + // bytes available to read + int available = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (available == 0) { + return 0; // buffer is empty + } + + int n = 0; + + if (available > 0) { + // read pointer is before write pointer, all available bytes can be read + n = available; + *is_part = false; + + } else { + // read pointer is after write pointer, read bytes from read_ptr to end of the buffer + n = _message_buffer.size - _message_buffer.read_ptr; + *is_part = _message_buffer.write_ptr > 0; + } + + *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); + return n; +} + +void +Mavlink::message_buffer_mark_read(int n) +{ + _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; +} + +void +Mavlink::pass_message(mavlink_message_t *msg) +{ + if (_passing_on) { + /* size is 8 bytes plus variable payload */ + int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; + pthread_mutex_lock(&_message_buffer_mutex); + message_buffer_write(msg, size); + pthread_mutex_unlock(&_message_buffer_mutex); + } +} + + + int Mavlink::task_main(int argc, char *argv[]) { @@ -1632,7 +1769,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpv")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1672,6 +1809,14 @@ Mavlink::task_main(int argc, char *argv[]) break; + case 'f': + _forwarding_on = true; + break; + + case 'p': + _passing_on = true; + break; + case 'v': _verbose = true; break; @@ -1740,6 +1885,17 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); + /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ + if (_passing_on) { + /* initialize message buffer if multiplexing is on */ + if (OK != message_buffer_init(500)) { + errx(1, "can't allocate message buffer, exiting"); + } + + /* initialize message buffer mutex */ + pthread_mutex_init(&_message_buffer_mutex, NULL); + } + /* create the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); @@ -1884,6 +2040,37 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* pass messages from other UARTs */ + if (_passing_on) { + + bool is_part; + void *read_ptr; + + /* guard get ptr by mutex */ + pthread_mutex_lock(&_message_buffer_mutex); + int available = message_buffer_get_ptr(&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + if (available > 0) { + /* write first part of buffer */ + _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + message_buffer_mark_read(available); + + /* write second part of buffer if there is some */ + if (is_part) { + /* guard get ptr by mutex */ + pthread_mutex_lock(&_message_buffer_mutex); + available = message_buffer_get_ptr(&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + message_buffer_mark_read(available); + } + } + } + + + perf_end(_loop_perf); } @@ -1928,6 +2115,10 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ close(_mavlink_fd); + if (_passing_on) { + message_buffer_destroy(); + pthread_mutex_destroy(&_message_buffer_mutex); + } /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -2067,7 +2258,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5a118a0ad..4f9a53a5b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -138,6 +138,8 @@ public: static bool instance_exists(const char *device_name, Mavlink *self); + static void forward_message(mavlink_message_t *msg, Mavlink *self); + static int get_uart_fd(unsigned index); int get_uart_fd(); @@ -153,10 +155,12 @@ public: void set_mode(enum MAVLINK_MODE); enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _hil_enabled; }; + bool get_hil_enabled() { return _hil_enabled; } bool get_flow_control_enabled() { return _flow_control_enabled; } + bool get_forwarding_on() { return _forwarding_on; } + /** * Handle waypoint related messages. */ @@ -234,6 +238,8 @@ private: mavlink_wpm_storage *_wpm; bool _verbose; + bool _forwarding_on; + bool _passing_on; int _uart_fd; int _baudrate; int _datarate; @@ -252,6 +258,16 @@ private: bool _flow_control_enabled; + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; + mavlink_message_buffer _message_buffer; + + pthread_mutex_t _message_buffer_mutex; + /** * Send one parameter. * @@ -315,6 +331,22 @@ private: int configure_stream(const char *stream_name, const float rate); void configure_stream_threadsafe(const char *stream_name, const float rate); + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty(); + + bool message_buffer_write(void *ptr, int size); + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n); + + void pass_message(mavlink_message_t *msg); + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4ca3840d4..2b5d65080 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1265,11 +1265,13 @@ protected: || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + /* XXX TODO: get param for system ID */ + mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + /* XXX TODO: get param for system ID */ + mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f6f5e4848..8e4e5f0d4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -880,6 +880,11 @@ MavlinkReceiver::receive_thread(void *arg) /* handle packet with parameter component */ _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); + + if (_mavlink->get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(&msg, _mavlink); + } } } } -- cgit v1.2.3 From 2cd4648a8e6df65ad953ffe06426c83476b7f835 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 7 Apr 2014 14:38:16 +0200 Subject: mavlink: in normal mode transmit position setpoint and roll-pitch-yaw-thrust setpoint --- src/modules/mavlink/mavlink_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c5055939e..bb73b8088 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1939,6 +1939,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); + configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); break; case MAVLINK_MODE_CAMERA: -- cgit v1.2.3 From a66dcbf7e92a1028765f6c5c9a545945ce1a5dc3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 17:17:56 +0400 Subject: mavlink: publish SYS_STATUS at constant rate, don't look at update() result --- src/modules/mavlink/mavlink_messages.cpp | 31 +++++++++++++++---------------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2b5d65080..6b543d44f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -262,22 +262,21 @@ protected: void send(const hrt_abstime t) { - if (status_sub->update(t)) { - mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); - } + status_sub->update(t); + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); } }; -- cgit v1.2.3 From 3dd64086e49192aabc020b50f48d68233bad392f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:42:19 +0200 Subject: commander: put unsupported warning back in place --- src/modules/commander/commander.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 27d5e4260..662b98b35 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -587,15 +587,15 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; default: - /* warn about unsupported commands */ + /* Warn about unsupported commands, this makes sense because only commands + * to this component ID (or all) are passed by mavlink. */ answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - /* silently ignore unsupported commands, maybe they are passed on over mavlink */ -// if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -// /* already warned about unsupported commands in "default" case */ -// answer_command(*cmd, result); -// } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* already warned about unsupported commands in "default" case */ + answer_command(*cmd, result); + } /* send any requested ACKs */ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -- cgit v1.2.3 From 01d9d482c3237250420e182e0aedd409c365f848 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:42:48 +0200 Subject: mavlink: use LL_FOREACH --- src/modules/mavlink/mavlink_main.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bb73b8088..3d897431a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -401,14 +401,12 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) { - Mavlink *inst = ::_mavlink_instances; - - while (inst != nullptr) { - /* don't broadcast to itself */ + + Mavlink *inst; + LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { inst->pass_message(msg); } - inst = inst->next; } } -- cgit v1.2.3 From d662fa4c14676aac26603e55c71c04a2dd29503a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:44:01 +0200 Subject: Send camera command to all, use own sysid --- src/modules/mavlink/mavlink_messages.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6b543d44f..45b86a497 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1264,13 +1264,11 @@ protected: || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - /* XXX TODO: get param for system ID */ - mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - /* XXX TODO: get param for system ID */ - mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; -- cgit v1.2.3 From fefe19a7b9d053265a55862cbbbf16978ca38846 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 11:03:23 +0200 Subject: gps driver: fake mode: lower eph and epv values in order to convince the commander that the gps signal is valid --- src/drivers/gps/gps.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index c72f692d7..a902bdf2f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -282,8 +282,8 @@ GPS::task_main() _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 10.0f; - _report.epv_m = 10.0f; + _report.eph_m = 3.0f; + _report.epv_m = 7.0f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; -- cgit v1.2.3 From 1c49f132a43cc8521fc0a9395b03911400c91435 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 15:15:06 +0200 Subject: mavlink: accessor for _mavlink_fd --- src/modules/mavlink/mavlink_main.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 4f9a53a5b..9941a5f99 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -200,6 +200,8 @@ public: bool _task_should_exit; /**< if true, mavlink task should exit */ + int get_mavlink_fd() { return _mavlink_fd; } + protected: Mavlink *next; -- cgit v1.2.3 From 03a3b1d67127af8681275a50e5604ea1b60814ea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:06:52 +0200 Subject: commander: handle_command: filter commands by sysid and componentid --- src/modules/commander/commander.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 662b98b35..e6de58563 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -395,6 +395,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; bool ret = false; + /* only handle commands that are meant to be handled by this system and component */ + if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) { + return false; + } + /* only handle high-priority commands here */ /* request to set different system mode */ -- cgit v1.2.3 From b2bc8c1f08fb728ca2b80e7df6c3288b3fda4e3b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:08:35 +0200 Subject: mavlink: COMMAND_LONG stream: listen to vehicle_command uorb topic and send mavlink_msg_command_long --- src/modules/mavlink/mavlink_messages.cpp | 46 ++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 45b86a497..4d7b9160d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1273,6 +1273,51 @@ protected: } }; +class MavlinkStreamCommandLong : public MavlinkStream +{ +public: + const char *get_name() + { + return "COMMAND_LONG"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamCommandLong(); + } + +private: + MavlinkOrbSubscription *vehicle_command_sub; + struct vehicle_command_s *vehicle_command; + +protected: + void subscribe(Mavlink *mavlink) + { + vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); + vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (vehicle_command_sub->update(t)) { + if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) { + mavlink_msg_command_long_send(_channel, + vehicle_command->target_system, + vehicle_command->target_component, + vehicle_command->command, + vehicle_command->confirmation, + vehicle_command->param1, + vehicle_command->param2, + vehicle_command->param3, + vehicle_command->param4, + vehicle_command->param5, + vehicle_command->param6, + vehicle_command->param7); + } + } + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1299,5 +1344,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamCommandLong(), nullptr }; -- cgit v1.2.3 From 93227f9200ee3369f0d2a3aa2fa7bb2a738e18c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:29:11 +0200 Subject: commander: handle_command: do not filter command if componentid == 0 --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e6de58563..e7cf2b3fa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -396,7 +396,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe bool ret = false; /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) { + if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } -- cgit v1.2.3 From b603b002bf4d26c35b0fb10f5192532e43fa57a0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Apr 2014 22:23:18 +0400 Subject: position_estimator_inav: make land detector more sensitive to LANDED -> IN AIR transitions --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 +- src/modules/position_estimator_inav/position_estimator_inav_params.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 368fa6ee2..763b87563 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -763,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { - if (alt_disp2 > land_disp2 && thrust > params.land_thr) { + if (alt_disp2 > land_disp2 || thrust > params.land_thr) { landed = false; landed_time = 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index b71f9472f..dcad5c03b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); -PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); +PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) { -- cgit v1.2.3 From 839fa1371de96b4647d4ace6c4dfab49d4d97af1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Apr 2014 23:28:52 +0400 Subject: mavlink: commands stream implemented --- src/modules/mavlink/mavlink_commands.cpp | 73 ++++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_commands.h | 65 ++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.cpp | 6 +++ src/modules/mavlink/module.mk | 3 +- 4 files changed, 146 insertions(+), 1 deletion(-) create mode 100644 src/modules/mavlink/mavlink_commands.cpp create mode 100644 src/modules/mavlink/mavlink_commands.h diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp new file mode 100644 index 000000000..1c1e097a4 --- /dev/null +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_commands.cpp + * Mavlink commands stream implementation. + * + * @author Anton Babushkin + */ + +#include "mavlink_commands.h" + +MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel) +{ + _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); + _cmd = (struct vehicle_command_s *)_cmd_sub->get_data(); +} + +MavlinkCommandsStream::~MavlinkCommandsStream() +{ +} + +void +MavlinkCommandsStream::update(const hrt_abstime t) +{ + if (_cmd_sub->update(t)) { + /* only send commands for other systems/components */ + if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) { + mavlink_msg_command_long_send(_channel, + _cmd->target_system, + _cmd->target_component, + _cmd->command, + _cmd->confirmation, + _cmd->param1, + _cmd->param2, + _cmd->param3, + _cmd->param4, + _cmd->param5, + _cmd->param6, + _cmd->param7); + } + } +} diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h new file mode 100644 index 000000000..6255d65df --- /dev/null +++ b/src/modules/mavlink/mavlink_commands.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_commands.h + * Mavlink commands stream definition. + * + * @author Anton Babushkin + */ + +#ifndef MAVLINK_COMMANDS_H_ +#define MAVLINK_COMMANDS_H_ + +#include +#include + +class Mavlink; +class MavlinkCommansStream; + +#include "mavlink_main.h" + +class MavlinkCommandsStream +{ +private: + MavlinkOrbSubscription *_cmd_sub; + struct vehicle_command_s *_cmd; + mavlink_channel_t _channel; + +public: + MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); + ~MavlinkCommandsStream(); + void update(const hrt_abstime t); +}; + +#endif /* MAVLINK_COMMANDS_H_ */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3d897431a..1ed3f4001 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -81,6 +81,7 @@ #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" +#include "mavlink_commands.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -1920,6 +1921,8 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); + MavlinkCommandsStream commands_stream(this, _channel); + /* add default streams depending on mode and intervals depending on datarate */ float rate_mult = _datarate / 1000.0f; @@ -1982,6 +1985,9 @@ Mavlink::task_main(int argc, char *argv[]) set_hil_enabled(status->hil_state == HIL_STATE_ON); } + /* update commands stream */ + commands_stream.update(t); + /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f09efa2e6..515fbfadc 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -42,6 +42,7 @@ SRCS += mavlink_main.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ - mavlink_rate_limiter.cpp + mavlink_rate_limiter.cpp \ + mavlink_commands.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink -- cgit v1.2.3 From 84aa96cf235e591fee13b55ec295db2b8b667c4d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Apr 2014 23:29:40 +0400 Subject: mavlink: minor comments and formatting fixes --- src/modules/mavlink/mavlink_messages.cpp | 2 -- src/modules/mavlink/mavlink_stream.h | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4d7b9160d..bb8bba037 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1252,8 +1252,6 @@ protected: { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); - - } void send(const hrt_abstime t) diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 135e1bce0..def40d9ad 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mavlink_stream.cpp + * @file mavlink_stream.h * Mavlink messages stream definition. * * @author Anton Babushkin -- cgit v1.2.3 From a96d83e4ec48dd11634709d449e8df2ea30146d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Apr 2014 21:40:05 +0200 Subject: Revert "Merge pull request #816 from PX4/mavlink_commandlongstream" This reverts commit 00ef10f307d3c4a262a15ab5747d854eb4c568d5, reversing changes made to d55e64d1e54542762510387a22897f504c68a5a6. --- src/modules/mavlink/mavlink_messages.cpp | 46 -------------------------------- 1 file changed, 46 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index bb8bba037..47603b390 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1271,51 +1271,6 @@ protected: } }; -class MavlinkStreamCommandLong : public MavlinkStream -{ -public: - const char *get_name() - { - return "COMMAND_LONG"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamCommandLong(); - } - -private: - MavlinkOrbSubscription *vehicle_command_sub; - struct vehicle_command_s *vehicle_command; - -protected: - void subscribe(Mavlink *mavlink) - { - vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); - vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (vehicle_command_sub->update(t)) { - if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) { - mavlink_msg_command_long_send(_channel, - vehicle_command->target_system, - vehicle_command->target_component, - vehicle_command->command, - vehicle_command->confirmation, - vehicle_command->param1, - vehicle_command->param2, - vehicle_command->param3, - vehicle_command->param4, - vehicle_command->param5, - vehicle_command->param6, - vehicle_command->param7); - } - } - } -}; - MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1342,6 +1297,5 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), - new MavlinkStreamCommandLong(), nullptr }; -- cgit v1.2.3 From 746c5086b03c642cc7d5047c9928c26fc8aac5f2 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:11:27 +0200 Subject: Added VICON logging, finally. --- src/modules/sdlog2/sdlog2.c | 30 +++++++++++++++++++----------- src/modules/sdlog2/sdlog2_messages.h | 12 ++++++++++++ 2 files changed, 31 insertions(+), 11 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 4b2d01c71..a664c9467 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -226,11 +226,11 @@ sdlog2_usage(const char *reason) } errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t\n" - "\t-r\tLog rate in Hz, 0 means unlimited rate\n" - "\t-b\tLog buffer size in KiB, default is 8\n" - "\t-e\tEnable logging by default (if not, can be started by command)\n" - "\t-a\tLog only when armed (can be still overriden by command)\n" - "\t-t\tUse date/time for naming log directories and files\n"); + "\t-r\tLog rate in Hz, 0 means unlimited rate\n" + "\t-b\tLog buffer size in KiB, default is 8\n" + "\t-e\tEnable logging by default (if not, can be started by command)\n" + "\t-a\tLog only when armed (can be still overriden by command)\n" + "\t-t\tUse date/time for naming log directories and files\n"); } /** @@ -257,11 +257,11 @@ int sdlog2_main(int argc, char *argv[]) main_thread_should_exit = false; deamon_task = task_spawn_cmd("sdlog2", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 3000, - sdlog2_thread_main, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT - 30, + 3000, + sdlog2_thread_main, + (const char **)argv); exit(0); } @@ -833,6 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TELE_s log_TELE; struct log_ESTM_s log_ESTM; struct log_PWR_s log_PWR; + struct log_VICON_s log_VICON; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1163,7 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VICON POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { - // TODO not implemented yet + log_msg.msg_type = LOG_VICON_MSG; + log_msg.body.log_VICON.x = buf.vicon_pos.x; + log_msg.body.log_VICON.y = buf.vicon_pos.y; + log_msg.body.log_VICON.z = buf.vicon_pos.z; + log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch; + log_msg.body.log_VICON.roll = buf.vicon_pos.roll; + log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VICON); } /* --- FLOW --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 4b70e55c9..ca4346a2e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -302,6 +302,17 @@ struct log_PWR_s { uint8_t high_power_rail_overcurrent; }; +/* --- VICON - VICON POSITION --- */ +#define LOG_VICON_MSG 25 +struct log_VICON_s { + float x; + float y; + float z; + float roll; + float pitch; + float yaw; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -352,6 +363,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), + LOG_FORMAT(VICON, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From 18e47c0945562a7599aabce4469cb3c41874e055 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:21:07 +0200 Subject: Tabs tabs tabs --- src/modules/sdlog2/sdlog2.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a664c9467..12cd90d21 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1164,14 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VICON POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { - log_msg.msg_type = LOG_VICON_MSG; - log_msg.body.log_VICON.x = buf.vicon_pos.x; - log_msg.body.log_VICON.y = buf.vicon_pos.y; - log_msg.body.log_VICON.z = buf.vicon_pos.z; - log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch; - log_msg.body.log_VICON.roll = buf.vicon_pos.roll; - log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw; - LOGBUFFER_WRITE_AND_COUNT(VICON); + log_msg.msg_type = LOG_VICON_MSG; + log_msg.body.log_VICON.x = buf.vicon_pos.x; + log_msg.body.log_VICON.y = buf.vicon_pos.y; + log_msg.body.log_VICON.z = buf.vicon_pos.z; + log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch; + log_msg.body.log_VICON.roll = buf.vicon_pos.roll; + log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VICON); } /* --- FLOW --- */ -- cgit v1.2.3 From df0ab0ecd0bd462a63f6958354e96d65346db4ce Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:25:33 +0200 Subject: VICON -> VICN --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index ca4346a2e..a229f860c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -363,7 +363,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), - LOG_FORMAT(VICON, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From 99ab001a5a0d7f6cebcfaa5acc86ab1dba57c876 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:28:00 +0200 Subject: VICON -> VICN really this time. --- src/modules/sdlog2/sdlog2.c | 18 +++++++++--------- src/modules/sdlog2/sdlog2_messages.h | 6 +++--- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 12cd90d21..cba39c5b1 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -833,7 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TELE_s log_TELE; struct log_ESTM_s log_ESTM; struct log_PWR_s log_PWR; - struct log_VICON_s log_VICON; + struct log_VICN_s log_VICN; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1164,14 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VICON POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { - log_msg.msg_type = LOG_VICON_MSG; - log_msg.body.log_VICON.x = buf.vicon_pos.x; - log_msg.body.log_VICON.y = buf.vicon_pos.y; - log_msg.body.log_VICON.z = buf.vicon_pos.z; - log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch; - log_msg.body.log_VICON.roll = buf.vicon_pos.roll; - log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw; - LOGBUFFER_WRITE_AND_COUNT(VICON); + log_msg.msg_type = LOG_VICN_MSG; + log_msg.body.log_VICN.x = buf.vicon_pos.x; + log_msg.body.log_VICN.y = buf.vicon_pos.y; + log_msg.body.log_VICN.z = buf.vicon_pos.z; + log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch; + log_msg.body.log_VICN.roll = buf.vicon_pos.roll; + log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VICN); } /* --- FLOW --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a229f860c..e38681a39 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -302,9 +302,9 @@ struct log_PWR_s { uint8_t high_power_rail_overcurrent; }; -/* --- VICON - VICON POSITION --- */ -#define LOG_VICON_MSG 25 -struct log_VICON_s { +/* --- VICN - VICON POSITION --- */ +#define LOG_VICN_MSG 25 +struct log_VICN_s { float x; float y; float z; -- cgit v1.2.3 From b96669a5314198b88e406fc02940ed2ced291f2b Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:58:18 +0200 Subject: Logging changes, baud rate changes. --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index c5aef8273..d6fbe8c4f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if hw_ver compare PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 8 -t + sdlog2 start -r 50 -a -b 1 -t -e else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0cb6d281a..a62f67718 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -410,7 +410,7 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" + set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 115200" # Exit from nsh to free port for mavlink set EXIT_ON_END yes -- cgit v1.2.3 From 665a2d6a92827e27213d596df7383061636c5dde Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Apr 2014 18:39:23 +0200 Subject: mavlink and mission topic: added reading in DO_JUMP mission items --- src/modules/mavlink/mavlink_main.cpp | 10 +++++++++- src/modules/uORB/topics/mission.h | 4 +++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ed3f4001..2dba567b0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -863,7 +863,10 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item case MAV_CMD_NAV_TAKEOFF: mission_item->pitch_min = mavlink_mission_item->param2; break; - + case MAV_CMD_DO_JUMP: + mission_item->do_jump_mission_index = mavlink_mission_item->param1; + /* TODO: also save param2 (repeat count) */ + break; default: mission_item->acceptance_radius = mavlink_mission_item->param2; break; @@ -896,6 +899,11 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ mavlink_mission_item->param2 = mission_item->pitch_min; break; + case NAV_CMD_DO_JUMP: + mavlink_mission_item->param1 = mission_item->do_jump_mission_index; + /* TODO: also save param2 (repeat count) */ + break; + default: mavlink_mission_item->param2 = mission_item->acceptance_radius; break; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 9594c4c6a..1ccdb7181 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -58,7 +58,8 @@ enum NAV_CMD { NAV_CMD_LAND=21, NAV_CMD_TAKEOFF=22, NAV_CMD_ROI=80, - NAV_CMD_PATHPLANNING=81 + NAV_CMD_PATHPLANNING=81, + NAV_CMD_DO_JUMP=177 }; enum ORIGIN { @@ -91,6 +92,7 @@ struct mission_item_s { float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ bool autocontinue; /**< true if next waypoint should follow after this one */ enum ORIGIN origin; /**< where the waypoint has been generated */ + int do_jump_mission_index; /**< the mission index that we want to jump to */ }; struct mission_s -- cgit v1.2.3 From 705b10fcafcb04440edc306cc07c7430e89ad788 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Apr 2014 18:39:42 +0200 Subject: navigator: support DO_JUMP misison items --- src/modules/navigator/navigator_mission.cpp | 94 +++++++++++++++++------------ src/modules/navigator/navigator_mission.h | 2 + 2 files changed, 56 insertions(+), 40 deletions(-) diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index 72dddebfe..ac7e604ef 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -142,22 +142,19 @@ Mission::next_mission_available() int Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index) { + int ret = ERROR; + /* try onboard mission first */ if (current_onboard_mission_available()) { - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; + ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &_current_onboard_mission_index, new_mission_item); + if (ret == OK) { + _current_mission_type = MISSION_TYPE_ONBOARD; + *onboard = true; + *index = _current_onboard_mission_index; } - _current_mission_type = MISSION_TYPE_ONBOARD; - *onboard = true; - *index = _current_onboard_mission_index; - - /* otherwise fallback to offboard */ - + /* otherwise fallback to offboard */ } else if (current_offboard_mission_available()) { dm_item_t dm_current; @@ -169,42 +166,34 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - _current_mission_type = MISSION_TYPE_NONE; - return ERROR; + ret = get_mission_item(dm_current, &_current_offboard_mission_index, new_mission_item); + if (ret == OK) { + _current_mission_type = MISSION_TYPE_OFFBOARD; + *onboard = false; + *index = _current_offboard_mission_index; } - _current_mission_type = MISSION_TYPE_OFFBOARD; - *onboard = false; - *index = _current_offboard_mission_index; - } else { /* happens when no more mission items can be added as a next item */ _current_mission_type = MISSION_TYPE_NONE; - return ERROR; + ret == ERROR; } - return OK; + return ret; } int Mission::get_next_mission_item(struct mission_item_s *new_mission_item) { + int ret = ERROR; + /* try onboard mission first */ if (next_onboard_mission_available()) { - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } - - /* otherwise fallback to offboard */ + unsigned next_onboard_mission_index = _current_onboard_mission_index + 1; + ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &next_onboard_mission_index, new_mission_item); + /* otherwise fallback to offboard */ } else if (next_offboard_mission_available()) { dm_item_t dm_current; @@ -216,19 +205,14 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item) dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } + unsigned next_offboard_mission_index = _current_offboard_mission_index + 1; + ret = get_mission_item(dm_current, &next_offboard_mission_index, new_mission_item); } else { /* happens when no more mission items can be added as a next item */ - return ERROR; + ret = ERROR; } - - return OK; + return ret; } @@ -316,3 +300,33 @@ Mission::publish_mission_result() /* reset reached bool */ _mission_result.mission_reached = false; } + +int +Mission::get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item) +{ + /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + for (int i=0; i<10; i++) { + + const ssize_t len = sizeof(struct mission_item_s); + + /* read mission item from datamanager */ + if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + + /* check for DO_JUMP item */ + if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; + } else { + /* if it's not a DO_JUMP, then we were successful */ + return OK; + } + } + + /* we have given up, we don't want to cycle forever */ + warnx("ERROR: cycling through mission items without success"); + return ERROR; +} diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index 2bd4da82e..fef145410 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -77,6 +77,8 @@ public: void publish_mission_result(); private: + int get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item); + bool current_onboard_mission_available(); bool current_offboard_mission_available(); bool next_onboard_mission_available(); -- cgit v1.2.3 From 5c996065e2ddbbfa3776390163435afa52a4b37d Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 09:08:37 +0200 Subject: Added vicon stream. --- src/modules/mavlink/mavlink_messages.cpp | 42 ++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 47603b390..37929edac 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -640,6 +640,47 @@ protected: }; + +class MavlinkStreamViconPositionEstimate : public MavlinkStream +{ +public: + const char *get_name() + { + return "VICON_POSITION_ESTIMATE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } + +private: + MavlinkOrbSubscription *pos_sub; + struct vehicle_vicon_position_s *pos; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (pos_sub->update(t)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); + } + } +}; + + class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { public: @@ -1297,5 +1338,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamViconPositionEstimate(), nullptr }; -- cgit v1.2.3 From 32c57294d80980c88dfd611cf8a9c34aefc6dcab Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 09:08:47 +0200 Subject: Set startup script to do mavlinks. --- ROMFS/px4fmu_common/init.d/rcS | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a62f67718..f0bdf32f8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -410,7 +410,7 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 115200" + set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 57600" # Exit from nsh to free port for mavlink set EXIT_ON_END yes @@ -421,8 +421,16 @@ then fi fi + # Main mavlink mavlink start $MAVLINK_FLAGS - + # Optical Flow + mavlink start -d /dev/ttyS3 -m custom -b 115200 + # Android board + #mavlink start -d /dev/ttyS2 -m custom -b 115200 + #mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -r 5 + #mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 + #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 50 + # # Start the datamanager # -- cgit v1.2.3 From e09aed917d0f301658cb7ed97d04c19a08a5482a Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:26:15 +0200 Subject: Finished adding a '-w' option. --- src/modules/mavlink/mavlink_main.cpp | 37 +++++++++++++++++++++----------- src/modules/mavlink/mavlink_main.h | 12 +++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 4 ++++ 3 files changed, 40 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2dba567b0..10d781a56 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -167,12 +167,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { if (buf_free == 0) { if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { warnx("DISABLING HARDWARE FLOW CONTROL"); instance->enable_flow_control(false); @@ -186,12 +186,17 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } } - ssize_t ret = write(uart, ch, desired); - - if (ret != desired) { - // XXX do something here, but change to using FIONWRITE and OS buf size for detection + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (instance->should_transmit()) { + ssize_t ret = write(uart, ch, desired); + if (ret != desired) { + // XXX do something here, but change to using FIONWRITE and OS buf size for detection + } } + + } static void usage(void); @@ -204,6 +209,8 @@ Mavlink::Mavlink() : _task_running(false), _hil_enabled(false), _is_usb_uart(false), + _wait_to_transmit(false), + _received_messages(false), _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), @@ -1776,7 +1783,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:fpv")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1828,6 +1835,10 @@ Mavlink::task_main(int argc, char *argv[]) _verbose = true; break; + case 'w': + _wait_to_transmit = true; + break; + default: err_flag = true; break; @@ -2172,11 +2183,11 @@ Mavlink::start(int argc, char *argv[]) // task - start_helper() only returns // when the started task exits. task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); // Ensure that this shell command // does not return before the instance @@ -2272,7 +2283,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 9941a5f99..2c1826139 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -202,6 +202,14 @@ public: int get_mavlink_fd() { return _mavlink_fd; } + + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + protected: Mavlink *next; @@ -216,6 +224,8 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ @@ -270,6 +280,8 @@ private: pthread_mutex_t _message_buffer_mutex; + + /** * Send one parameter. * diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8e4e5f0d4..9180ec5e6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -179,6 +179,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; } } + + /* If we've received a valid message, mark the flag indicating so. + This is used in the '-w' command-line flag. */ + _mavlink->set_has_received_messages(true); } void -- cgit v1.2.3 From 7d38af81d7c3f93dc5a03d1fe8082b8a9ac7f109 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:26:33 +0200 Subject: Fixed final parameters in rcS --- ROMFS/px4fmu_common/init.d/rcS | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0bdf32f8..e37cab04c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -422,14 +422,14 @@ then fi # Main mavlink - mavlink start $MAVLINK_FLAGS + mavlink start -d /dev/ttyS0 -b 57600 + mavlink stream -d /dev/ttyS0 -s OPTICAL_FLOW -r 10 # Optical Flow mavlink start -d /dev/ttyS3 -m custom -b 115200 # Android board - #mavlink start -d /dev/ttyS2 -m custom -b 115200 - #mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -r 5 - #mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 - #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 50 + mavlink start -d /dev/ttyS2 -m custom -b 115200 -w + mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 + #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 10 # # Start the datamanager @@ -453,7 +453,7 @@ then sh /etc/init.d/rc.logging echo "[init] Start GPS" - gps start + #gps start fi # -- cgit v1.2.3 From 0ed8a6d2da0ffd7f32d86be462ed3fea6fc7dcb3 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:27:35 +0200 Subject: Reverted rcS --- ROMFS/px4fmu_common/init.d/rcS | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e37cab04c..0cb6d281a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -410,7 +410,7 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 57600" + set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" # Exit from nsh to free port for mavlink set EXIT_ON_END yes @@ -421,16 +421,8 @@ then fi fi - # Main mavlink - mavlink start -d /dev/ttyS0 -b 57600 - mavlink stream -d /dev/ttyS0 -s OPTICAL_FLOW -r 10 - # Optical Flow - mavlink start -d /dev/ttyS3 -m custom -b 115200 - # Android board - mavlink start -d /dev/ttyS2 -m custom -b 115200 -w - mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 - #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 10 - + mavlink start $MAVLINK_FLAGS + # # Start the datamanager # @@ -453,7 +445,7 @@ then sh /etc/init.d/rc.logging echo "[init] Start GPS" - #gps start + gps start fi # -- cgit v1.2.3 From ff2e4732a0dd03121a0f6106fd9ad1b2dbeea527 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:30:00 +0200 Subject: Indentation. --- src/modules/mavlink/mavlink_main.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 2c1826139..66d82b471 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -203,12 +203,12 @@ public: int get_mavlink_fd() { return _mavlink_fd; } - /* Functions for waiting to start transmission until message received. */ - void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } - bool get_has_received_messages() { return _received_messages; } - void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } - bool get_wait_to_transmit() { return _wait_to_transmit; } - bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } protected: Mavlink *next; @@ -224,8 +224,8 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _is_usb_uart; /**< Port is USB */ - bool _wait_to_transmit; /**< Wait to transmit until received messages. */ - bool _received_messages; /**< Whether we've received valid mavlink messages. */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ -- cgit v1.2.3 From e35d4cd49181eef2c336cd0596f723a47bbf54ba Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:33:10 +0200 Subject: Reverted logging. --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- src/modules/mavlink/mavlink_main.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index d6fbe8c4f..c5aef8273 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if hw_ver compare PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 1 -t -e + sdlog2 start -r 50 -a -b 8 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 10d781a56..c3b59f940 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2183,11 +2183,11 @@ Mavlink::start(int argc, char *argv[]) // task - start_helper() only returns // when the started task exits. task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); // Ensure that this shell command // does not return before the instance -- cgit v1.2.3 From 7accde9b5bce299722384ebd62ab767331d8b73e Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:35:30 +0200 Subject: Whtespace. --- src/modules/mavlink/mavlink_main.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c3b59f940..072f16aae 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2183,11 +2183,11 @@ Mavlink::start(int argc, char *argv[]) // task - start_helper() only returns // when the started task exits. task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); // Ensure that this shell command // does not return before the instance -- cgit v1.2.3 From d6647d841a20574c5e30e1c976b6a4247616d382 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:37:58 +0200 Subject: More whitespace all the time. --- src/modules/mavlink/mavlink_messages.cpp | 180 +++++++++++++++---------------- src/modules/mavlink/mavlink_receiver.cpp | 6 +- 2 files changed, 93 insertions(+), 93 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 37929edac..2d1d92243 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -94,7 +94,7 @@ cm_uint16_from_m_float(float m) } void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -107,7 +107,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* arming state */ if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -163,8 +163,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* set system state */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; } else if (status->arming_state == ARMING_STATE_ARMED) { @@ -344,13 +344,13 @@ protected: } mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); } } }; @@ -420,14 +420,14 @@ protected: { if (att_sub->update(t)) { mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); } } }; @@ -534,16 +534,16 @@ protected: { if (gps_sub->update(t)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); } } }; @@ -586,15 +586,15 @@ protected: if (updated) { mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } } }; @@ -628,13 +628,13 @@ protected: { if (pos_sub->update(t)) { mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); } } }; @@ -644,40 +644,40 @@ protected: class MavlinkStreamViconPositionEstimate : public MavlinkStream { public: - const char *get_name() - { - return "VICON_POSITION_ESTIMATE"; - } + const char *get_name() + { + return "VICON_POSITION_ESTIMATE"; + } - MavlinkStream *new_instance() - { - return new MavlinkStreamViconPositionEstimate(); - } + MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_vicon_position_s *pos; + MavlinkOrbSubscription *pos_sub; + struct vehicle_vicon_position_s *pos; protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sub->update(t)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->roll, - pos->pitch, - pos->yaw); - } - } + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (pos_sub->update(t)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); + } + } }; @@ -869,10 +869,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } else { /* fixed wing: scale all channels except throttle -1 .. 1 @@ -897,10 +897,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } } } @@ -1175,12 +1175,12 @@ protected: { if (flow_sub->update(t)) { mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); } } }; @@ -1300,7 +1300,7 @@ protected: (void)status_sub->update(t); if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); @@ -1338,6 +1338,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), - new MavlinkStreamViconPositionEstimate(), + new MavlinkStreamViconPositionEstimate(), nullptr }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9180ec5e6..f5029652d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -180,9 +180,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } } - /* If we've received a valid message, mark the flag indicating so. - This is used in the '-w' command-line flag. */ - _mavlink->set_has_received_messages(true); + /* If we've received a valid message, mark the flag indicating so. + This is used in the '-w' command-line flag. */ + _mavlink->set_has_received_messages(true); } void -- cgit v1.2.3 From ebf0678333a91b405d8d26803a97ba18b64666dd Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:41:00 +0200 Subject: More whitespace all the time. --- src/modules/mavlink/mavlink_messages.cpp | 134 +++++++++++++++---------------- 1 file changed, 67 insertions(+), 67 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2d1d92243..648228e3b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -94,7 +94,7 @@ cm_uint16_from_m_float(float m) } void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -107,7 +107,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* arming state */ if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -163,8 +163,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* set system state */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; } else if (status->arming_state == ARMING_STATE_ARMED) { @@ -344,13 +344,13 @@ protected: } mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); } } }; @@ -420,14 +420,14 @@ protected: { if (att_sub->update(t)) { mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); } } }; @@ -534,16 +534,16 @@ protected: { if (gps_sub->update(t)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); } } }; @@ -586,15 +586,15 @@ protected: if (updated) { mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } } }; @@ -628,13 +628,13 @@ protected: { if (pos_sub->update(t)) { mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); } } }; @@ -669,13 +669,13 @@ protected: { if (pos_sub->update(t)) { mavlink_msg_vicon_position_estimate_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->roll, - pos->pitch, - pos->yaw); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); } } }; @@ -869,10 +869,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } else { /* fixed wing: scale all channels except throttle -1 .. 1 @@ -897,10 +897,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } } } @@ -1175,12 +1175,12 @@ protected: { if (flow_sub->update(t)) { mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); } } }; @@ -1300,7 +1300,7 @@ protected: (void)status_sub->update(t); if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); -- cgit v1.2.3 From fde2878413141953e30e41aa9689c924c83e207f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Apr 2014 20:45:03 +0200 Subject: mavlink: Change to size optimization --- src/modules/mavlink/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 515fbfadc..c44354ff0 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -46,3 +46,5 @@ SRCS += mavlink_main.cpp \ mavlink_commands.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From d7c4867898184c5a884bcf144d7222e84dd00f28 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Apr 2014 09:41:23 +0200 Subject: mavlink: Updated protocol files --- .../mavlink/v1.0/ardupilotmega/ardupilotmega.h | 108 +--- .../mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h | 44 ++ .../mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h | 42 ++ .../ardupilotmega/mavlink_msg_airspeed_autocal.h | 54 ++ .../v1.0/ardupilotmega/mavlink_msg_ap_adc.h | 42 ++ .../ardupilotmega/mavlink_msg_compassmot_status.h | 329 ++++++++++ .../v1.0/ardupilotmega/mavlink_msg_data16.h | 34 + .../v1.0/ardupilotmega/mavlink_msg_data32.h | 34 + .../v1.0/ardupilotmega/mavlink_msg_data64.h | 34 + .../v1.0/ardupilotmega/mavlink_msg_data96.h | 34 + .../ardupilotmega/mavlink_msg_digicam_configure.h | 52 ++ .../ardupilotmega/mavlink_msg_digicam_control.h | 50 ++ .../ardupilotmega/mavlink_msg_fence_fetch_point.h | 36 ++ .../v1.0/ardupilotmega/mavlink_msg_fence_point.h | 42 ++ .../v1.0/ardupilotmega/mavlink_msg_fence_status.h | 38 ++ .../v1.0/ardupilotmega/mavlink_msg_hwstatus.h | 34 + .../v1.0/ardupilotmega/mavlink_msg_limits_status.h | 48 ++ .../v1.0/ardupilotmega/mavlink_msg_meminfo.h | 34 + .../ardupilotmega/mavlink_msg_mount_configure.h | 42 ++ .../v1.0/ardupilotmega/mavlink_msg_mount_control.h | 42 ++ .../v1.0/ardupilotmega/mavlink_msg_mount_status.h | 40 ++ .../mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h | 44 ++ .../ardupilotmega/mavlink_msg_rally_fetch_point.h | 36 ++ .../v1.0/ardupilotmega/mavlink_msg_rally_point.h | 50 ++ .../v1.0/ardupilotmega/mavlink_msg_rangefinder.h | 34 + .../ardupilotmega/mavlink_msg_sensor_offsets.h | 54 ++ .../ardupilotmega/mavlink_msg_set_mag_offsets.h | 40 ++ .../v1.0/ardupilotmega/mavlink_msg_simstate.h | 52 ++ .../mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h | 36 ++ .../include/mavlink/v1.0/ardupilotmega/testsuite.h | 54 ++ .../include/mavlink/v1.0/ardupilotmega/version.h | 2 +- mavlink/include/mavlink/v1.0/autoquad/autoquad.h | 17 +- .../v1.0/autoquad/mavlink_msg_aq_telemetry_f.h | 72 +++ mavlink/include/mavlink/v1.0/autoquad/version.h | 2 +- mavlink/include/mavlink/v1.0/common/common.h | 124 +++- .../mavlink/v1.0/common/mavlink_msg_attitude.h | 44 ++ .../v1.0/common/mavlink_msg_attitude_quaternion.h | 46 ++ .../mavlink/v1.0/common/mavlink_msg_auth_key.h | 32 + .../v1.0/common/mavlink_msg_battery_status.h | 52 ++ .../common/mavlink_msg_change_operator_control.h | 36 ++ .../mavlink_msg_change_operator_control_ack.h | 36 ++ .../mavlink/v1.0/common/mavlink_msg_command_ack.h | 34 + .../mavlink/v1.0/common/mavlink_msg_command_long.h | 52 ++ .../mavlink/v1.0/common/mavlink_msg_data_stream.h | 36 ++ .../mavlink_msg_data_transmission_handshake.h | 44 ++ .../mavlink/v1.0/common/mavlink_msg_debug.h | 36 ++ .../mavlink/v1.0/common/mavlink_msg_debug_vect.h | 38 ++ .../v1.0/common/mavlink_msg_distance_sensor.h | 377 +++++++++++ .../v1.0/common/mavlink_msg_encapsulated_data.h | 32 + .../common/mavlink_msg_file_transfer_dir_list.h | 34 + .../v1.0/common/mavlink_msg_file_transfer_res.h | 34 + .../v1.0/common/mavlink_msg_file_transfer_start.h | 38 ++ .../v1.0/common/mavlink_msg_global_position_int.h | 48 ++ .../mavlink_msg_global_position_setpoint_int.h | 40 ++ .../mavlink_msg_global_vision_position_estimate.h | 44 ++ .../mavlink/v1.0/common/mavlink_msg_gps2_raw.h | 54 ++ .../v1.0/common/mavlink_msg_gps_global_origin.h | 36 ++ .../v1.0/common/mavlink_msg_gps_inject_data.h | 36 ++ .../mavlink/v1.0/common/mavlink_msg_gps_raw_int.h | 50 ++ .../mavlink/v1.0/common/mavlink_msg_gps_status.h | 40 ++ .../mavlink/v1.0/common/mavlink_msg_heartbeat.h | 42 ++ .../mavlink/v1.0/common/mavlink_msg_highres_imu.h | 60 ++ .../mavlink/v1.0/common/mavlink_msg_hil_controls.h | 52 ++ .../mavlink/v1.0/common/mavlink_msg_hil_gps.h | 56 ++ .../v1.0/common/mavlink_msg_hil_optical_flow.h | 46 ++ .../v1.0/common/mavlink_msg_hil_rc_inputs_raw.h | 58 ++ .../mavlink/v1.0/common/mavlink_msg_hil_sensor.h | 60 ++ .../mavlink/v1.0/common/mavlink_msg_hil_state.h | 62 ++ .../v1.0/common/mavlink_msg_hil_state_quaternion.h | 60 ++ .../v1.0/common/mavlink_msg_local_position_ned.h | 44 ++ ...k_msg_local_position_ned_system_global_offset.h | 44 ++ .../common/mavlink_msg_local_position_setpoint.h | 40 ++ .../mavlink/v1.0/common/mavlink_msg_log_data.h | 36 ++ .../mavlink/v1.0/common/mavlink_msg_log_entry.h | 40 ++ .../mavlink/v1.0/common/mavlink_msg_log_erase.h | 34 + .../v1.0/common/mavlink_msg_log_request_data.h | 40 ++ .../v1.0/common/mavlink_msg_log_request_end.h | 34 + .../v1.0/common/mavlink_msg_log_request_list.h | 38 ++ .../v1.0/common/mavlink_msg_manual_control.h | 42 ++ .../v1.0/common/mavlink_msg_manual_setpoint.h | 44 ++ .../mavlink/v1.0/common/mavlink_msg_memory_vect.h | 36 ++ .../mavlink/v1.0/common/mavlink_msg_mission_ack.h | 36 ++ .../v1.0/common/mavlink_msg_mission_clear_all.h | 34 + .../v1.0/common/mavlink_msg_mission_count.h | 36 ++ .../v1.0/common/mavlink_msg_mission_current.h | 32 + .../mavlink/v1.0/common/mavlink_msg_mission_item.h | 58 ++ .../v1.0/common/mavlink_msg_mission_item_reached.h | 32 + .../v1.0/common/mavlink_msg_mission_request.h | 36 ++ .../v1.0/common/mavlink_msg_mission_request_list.h | 34 + .../mavlink_msg_mission_request_partial_list.h | 38 ++ .../v1.0/common/mavlink_msg_mission_set_current.h | 36 ++ .../mavlink_msg_mission_write_partial_list.h | 38 ++ .../v1.0/common/mavlink_msg_named_value_float.h | 34 + .../v1.0/common/mavlink_msg_named_value_int.h | 34 + .../common/mavlink_msg_nav_controller_output.h | 46 ++ .../v1.0/common/mavlink_msg_omnidirectional_flow.h | 40 ++ .../mavlink/v1.0/common/mavlink_msg_optical_flow.h | 46 ++ .../v1.0/common/mavlink_msg_param_request_list.h | 34 + .../v1.0/common/mavlink_msg_param_request_read.h | 36 ++ .../mavlink/v1.0/common/mavlink_msg_param_set.h | 38 ++ .../mavlink/v1.0/common/mavlink_msg_param_value.h | 38 ++ .../include/mavlink/v1.0/common/mavlink_msg_ping.h | 38 ++ .../mavlink/v1.0/common/mavlink_msg_power_status.h | 257 ++++++++ .../mavlink/v1.0/common/mavlink_msg_radio_status.h | 44 ++ .../mavlink/v1.0/common/mavlink_msg_raw_imu.h | 50 ++ .../mavlink/v1.0/common/mavlink_msg_raw_pressure.h | 40 ++ .../mavlink/v1.0/common/mavlink_msg_rc_channels.h | 689 +++++++++++++++++++++ .../v1.0/common/mavlink_msg_rc_channels_override.h | 50 ++ .../v1.0/common/mavlink_msg_rc_channels_raw.h | 52 ++ .../v1.0/common/mavlink_msg_rc_channels_scaled.h | 52 ++ .../v1.0/common/mavlink_msg_request_data_stream.h | 40 ++ ...link_msg_roll_pitch_yaw_rates_thrust_setpoint.h | 40 ++ ...link_msg_roll_pitch_yaw_speed_thrust_setpoint.h | 40 ++ .../mavlink_msg_roll_pitch_yaw_thrust_setpoint.h | 40 ++ .../v1.0/common/mavlink_msg_safety_allowed_area.h | 44 ++ .../common/mavlink_msg_safety_set_allowed_area.h | 48 ++ .../mavlink/v1.0/common/mavlink_msg_scaled_imu.h | 50 ++ .../mavlink/v1.0/common/mavlink_msg_scaled_imu2.h | 50 ++ .../v1.0/common/mavlink_msg_scaled_pressure.h | 38 ++ .../v1.0/common/mavlink_msg_serial_control.h | 321 ++++++++++ .../v1.0/common/mavlink_msg_servo_output_raw.h | 50 ++ .../mavlink_msg_set_global_position_setpoint_int.h | 40 ++ .../common/mavlink_msg_set_gps_global_origin.h | 38 ++ .../mavlink_msg_set_local_position_setpoint.h | 44 ++ .../mavlink/v1.0/common/mavlink_msg_set_mode.h | 36 ++ .../common/mavlink_msg_set_quad_motors_setpoint.h | 40 ++ ..._msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h | 46 ++ ...link_msg_set_quad_swarm_roll_pitch_yaw_thrust.h | 40 ++ .../mavlink_msg_set_roll_pitch_yaw_speed_thrust.h | 42 ++ .../common/mavlink_msg_set_roll_pitch_yaw_thrust.h | 42 ++ .../v1.0/common/mavlink_msg_setpoint_6dof.h | 44 ++ .../v1.0/common/mavlink_msg_setpoint_8dof.h | 48 ++ .../mavlink/v1.0/common/mavlink_msg_sim_state.h | 72 +++ .../v1.0/common/mavlink_msg_state_correction.h | 48 ++ .../mavlink/v1.0/common/mavlink_msg_statustext.h | 32 + .../mavlink/v1.0/common/mavlink_msg_sys_status.h | 56 ++ .../mavlink/v1.0/common/mavlink_msg_system_time.h | 34 + .../mavlink/v1.0/common/mavlink_msg_vfr_hud.h | 42 ++ .../common/mavlink_msg_vicon_position_estimate.h | 44 ++ .../common/mavlink_msg_vision_position_estimate.h | 44 ++ .../common/mavlink_msg_vision_speed_estimate.h | 38 ++ mavlink/include/mavlink/v1.0/common/testsuite.h | 244 ++++++++ mavlink/include/mavlink/v1.0/common/version.h | 2 +- .../include/mavlink/v1.0/matrixpilot/matrixpilot.h | 17 +- .../v1.0/matrixpilot/mavlink_msg_airspeeds.h | 44 ++ .../v1.0/matrixpilot/mavlink_msg_altitudes.h | 44 ++ .../mavlink_msg_flexifunction_buffer_function.h | 42 ++ ...mavlink_msg_flexifunction_buffer_function_ack.h | 38 ++ .../mavlink_msg_flexifunction_command.h | 36 ++ .../mavlink_msg_flexifunction_command_ack.h | 34 + .../mavlink_msg_flexifunction_directory.h | 40 ++ .../mavlink_msg_flexifunction_directory_ack.h | 42 ++ .../mavlink_msg_flexifunction_read_req.h | 38 ++ .../matrixpilot/mavlink_msg_flexifunction_set.h | 34 + .../matrixpilot/mavlink_msg_serial_udb_extra_f13.h | 38 ++ .../matrixpilot/mavlink_msg_serial_udb_extra_f14.h | 52 ++ .../matrixpilot/mavlink_msg_serial_udb_extra_f15.h | 34 + .../matrixpilot/mavlink_msg_serial_udb_extra_f16.h | 34 + .../mavlink_msg_serial_udb_extra_f2_a.h | 86 +++ .../mavlink_msg_serial_udb_extra_f2_b.h | 96 +++ .../matrixpilot/mavlink_msg_serial_udb_extra_f4.h | 50 ++ .../matrixpilot/mavlink_msg_serial_udb_extra_f5.h | 42 ++ .../matrixpilot/mavlink_msg_serial_udb_extra_f6.h | 40 ++ .../matrixpilot/mavlink_msg_serial_udb_extra_f7.h | 42 ++ .../matrixpilot/mavlink_msg_serial_udb_extra_f8.h | 44 ++ mavlink/include/mavlink/v1.0/matrixpilot/version.h | 2 +- .../v1.0/pixhawk/mavlink_msg_attitude_control.h | 48 ++ .../v1.0/pixhawk/mavlink_msg_brief_feature.h | 44 ++ .../v1.0/pixhawk/mavlink_msg_image_available.h | 76 +++ .../pixhawk/mavlink_msg_image_trigger_control.h | 32 + .../v1.0/pixhawk/mavlink_msg_image_triggered.h | 54 ++ .../mavlink/v1.0/pixhawk/mavlink_msg_marker.h | 44 ++ .../v1.0/pixhawk/mavlink_msg_pattern_detected.h | 36 ++ .../v1.0/pixhawk/mavlink_msg_point_of_interest.h | 44 ++ .../mavlink_msg_point_of_interest_connection.h | 50 ++ .../mavlink_msg_position_control_setpoint.h | 40 ++ .../mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h | 44 ++ .../v1.0/pixhawk/mavlink_msg_set_cam_shutter.h | 42 ++ .../mavlink_msg_set_position_control_offset.h | 42 ++ .../v1.0/pixhawk/mavlink_msg_watchdog_command.h | 38 ++ .../v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h | 34 + .../pixhawk/mavlink_msg_watchdog_process_info.h | 38 ++ .../pixhawk/mavlink_msg_watchdog_process_status.h | 42 ++ mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h | 17 +- mavlink/include/mavlink/v1.0/pixhawk/version.h | 2 +- .../v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h | 34 + .../v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h | 34 + .../v1.0/sensesoar/mavlink_msg_filt_rot_vel.h | 32 + .../mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h | 34 + .../v1.0/sensesoar/mavlink_msg_obs_air_temp.h | 32 + .../v1.0/sensesoar/mavlink_msg_obs_air_velocity.h | 36 ++ .../v1.0/sensesoar/mavlink_msg_obs_attitude.h | 32 + .../mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h | 34 + .../v1.0/sensesoar/mavlink_msg_obs_position.h | 36 ++ .../mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h | 32 + .../v1.0/sensesoar/mavlink_msg_obs_velocity.h | 32 + .../mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h | 32 + .../mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h | 34 + .../mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h | 38 ++ mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h | 6 +- mavlink/include/mavlink/v1.0/sensesoar/version.h | 2 +- 201 files changed, 10148 insertions(+), 120 deletions(-) create mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index 2acb7965c..c887dc68a 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 0, 24, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 20, 24, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 0, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_AHRS2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS, MAVLINK_MESSAGE_INFO_AHRS2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -30,95 +30,6 @@ extern "C" { // ENUM DEFINITIONS -/** @brief Enumeration of possible mount operation modes */ -#ifndef HAVE_ENUM_MAV_MOUNT_MODE -#define HAVE_ENUM_MAV_MOUNT_MODE -enum MAV_MOUNT_MODE -{ - MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization | */ - MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM. | */ - MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ - MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ - MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ - MAV_MOUNT_MODE_ENUM_END=5, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_CMD -#define HAVE_ENUM_MAV_CMD -enum MAV_CMD -{ - MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ - MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ - MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_ENUM_END=501, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_FENCE_ACTION -#define HAVE_ENUM_FENCE_ACTION -enum FENCE_ACTION -{ - FENCE_ACTION_NONE=0, /* Disable fenced mode | */ - FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */ - FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */ - FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */ - FENCE_ACTION_ENUM_END=4, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_FENCE_BREACH -#define HAVE_ENUM_FENCE_BREACH -enum FENCE_BREACH -{ - FENCE_BREACH_NONE=0, /* No last fence breach | */ - FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */ - FENCE_BREACH_MAXALT=2, /* Breached maximum altitude | */ - FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */ - FENCE_BREACH_ENUM_END=4, /* | */ -}; -#endif - /** @brief */ #ifndef HAVE_ENUM_LIMITS_STATE #define HAVE_ENUM_LIMITS_STATE @@ -157,6 +68,18 @@ enum RALLY_FLAGS }; #endif +/** @brief */ +#ifndef HAVE_ENUM_PARACHUTE_ACTION +#define HAVE_ENUM_PARACHUTE_ACTION +enum PARACHUTE_ACTION +{ + PARACHUTE_DISABLE=0, /* Disable parachute release | */ + PARACHUTE_ENABLE=1, /* Enable parachute release | */ + PARACHUTE_RELEASE=2, /* Release parachute | */ + PARACHUTE_ACTION_ENUM_END=3, /* | */ +}; +#endif + #include "../common/common.h" // MAVLINK VERSION @@ -197,6 +120,7 @@ enum RALLY_FLAGS #include "./mavlink_msg_airspeed_autocal.h" #include "./mavlink_msg_rally_point.h" #include "./mavlink_msg_rally_fetch_point.h" +#include "./mavlink_msg_compassmot_status.h" #include "./mavlink_msg_ahrs2.h" #ifdef __cplusplus diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h index c3ead1140..50ddc79e5 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, #endif } +#if MAVLINK_MSG_ID_AHRS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ahrs_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, omegaIx); + _mav_put_float(buf, 4, omegaIy); + _mav_put_float(buf, 8, omegaIz); + _mav_put_float(buf, 12, accel_weight); + _mav_put_float(buf, 16, renorm_val); + _mav_put_float(buf, 20, error_rp); + _mav_put_float(buf, 24, error_yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN); +#endif +#else + mavlink_ahrs_t *packet = (mavlink_ahrs_t *)msgbuf; + packet->omegaIx = omegaIx; + packet->omegaIy = omegaIy; + packet->omegaIz = omegaIz; + packet->accel_weight = accel_weight; + packet->renorm_val = renorm_val; + packet->error_rp = error_rp; + packet->error_yaw = error_yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)packet, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)packet, MAVLINK_MSG_ID_AHRS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AHRS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h index f6fde9590..33e006688 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_ahrs2_send(mavlink_channel_t chan, float roll, fl #endif } +#if MAVLINK_MSG_ID_AHRS2_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ahrs2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +#else + mavlink_ahrs2_t *packet = (mavlink_ahrs2_t *)msgbuf; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->altitude = altitude; + packet->lat = lat; + packet->lng = lng; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)packet, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)packet, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AHRS2 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h index d046f2ad0..521831c8f 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h @@ -267,6 +267,60 @@ static inline void mavlink_msg_airspeed_autocal_send(mavlink_channel_t chan, flo #endif } +#if MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_airspeed_autocal_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#else + mavlink_airspeed_autocal_t *packet = (mavlink_airspeed_autocal_t *)msgbuf; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->diff_pressure = diff_pressure; + packet->EAS2TAS = EAS2TAS; + packet->ratio = ratio; + packet->state_x = state_x; + packet->state_y = state_y; + packet->state_z = state_z; + packet->Pax = Pax; + packet->Pby = Pby; + packet->Pcz = Pcz; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AIRSPEED_AUTOCAL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h index 821ce73e4..3c18e644e 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1 #endif } +#if MAVLINK_MSG_ID_AP_ADC_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ap_adc_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN); +#endif +#else + mavlink_ap_adc_t *packet = (mavlink_ap_adc_t *)msgbuf; + packet->adc1 = adc1; + packet->adc2 = adc2; + packet->adc3 = adc3; + packet->adc4 = adc4; + packet->adc5 = adc5; + packet->adc6 = adc6; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)packet, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)packet, MAVLINK_MSG_ID_AP_ADC_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AP_ADC UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h new file mode 100644 index 000000000..965545988 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h @@ -0,0 +1,329 @@ +// MESSAGE COMPASSMOT_STATUS PACKING + +#define MAVLINK_MSG_ID_COMPASSMOT_STATUS 177 + +typedef struct __mavlink_compassmot_status_t +{ + float current; ///< current (amps) + float CompensationX; ///< Motor Compensation X + float CompensationY; ///< Motor Compensation Y + float CompensationZ; ///< Motor Compensation Z + uint16_t throttle; ///< throttle (percent*10) + uint16_t interference; ///< interference (percent) +} mavlink_compassmot_status_t; + +#define MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN 20 +#define MAVLINK_MSG_ID_177_LEN 20 + +#define MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC 240 +#define MAVLINK_MSG_ID_177_CRC 240 + + + +#define MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS { \ + "COMPASSMOT_STATUS", \ + 6, \ + { { "current", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_compassmot_status_t, current) }, \ + { "CompensationX", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_compassmot_status_t, CompensationX) }, \ + { "CompensationY", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_compassmot_status_t, CompensationY) }, \ + { "CompensationZ", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_compassmot_status_t, CompensationZ) }, \ + { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_compassmot_status_t, throttle) }, \ + { "interference", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_compassmot_status_t, interference) }, \ + } \ +} + + +/** + * @brief Pack a compassmot_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param throttle throttle (percent*10) + * @param current current (amps) + * @param interference interference (percent) + * @param CompensationX Motor Compensation X + * @param CompensationY Motor Compensation Y + * @param CompensationZ Motor Compensation Z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_compassmot_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN]; + _mav_put_float(buf, 0, current); + _mav_put_float(buf, 4, CompensationX); + _mav_put_float(buf, 8, CompensationY); + _mav_put_float(buf, 12, CompensationZ); + _mav_put_uint16_t(buf, 16, throttle); + _mav_put_uint16_t(buf, 18, interference); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#else + mavlink_compassmot_status_t packet; + packet.current = current; + packet.CompensationX = CompensationX; + packet.CompensationY = CompensationY; + packet.CompensationZ = CompensationZ; + packet.throttle = throttle; + packet.interference = interference; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPASSMOT_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +} + +/** + * @brief Pack a compassmot_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param throttle throttle (percent*10) + * @param current current (amps) + * @param interference interference (percent) + * @param CompensationX Motor Compensation X + * @param CompensationY Motor Compensation Y + * @param CompensationZ Motor Compensation Z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_compassmot_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t throttle,float current,uint16_t interference,float CompensationX,float CompensationY,float CompensationZ) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN]; + _mav_put_float(buf, 0, current); + _mav_put_float(buf, 4, CompensationX); + _mav_put_float(buf, 8, CompensationY); + _mav_put_float(buf, 12, CompensationZ); + _mav_put_uint16_t(buf, 16, throttle); + _mav_put_uint16_t(buf, 18, interference); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#else + mavlink_compassmot_status_t packet; + packet.current = current; + packet.CompensationX = CompensationX; + packet.CompensationY = CompensationY; + packet.CompensationZ = CompensationZ; + packet.throttle = throttle; + packet.interference = interference; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPASSMOT_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +} + +/** + * @brief Encode a compassmot_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param compassmot_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_compassmot_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_compassmot_status_t* compassmot_status) +{ + return mavlink_msg_compassmot_status_pack(system_id, component_id, msg, compassmot_status->throttle, compassmot_status->current, compassmot_status->interference, compassmot_status->CompensationX, compassmot_status->CompensationY, compassmot_status->CompensationZ); +} + +/** + * @brief Encode a compassmot_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param compassmot_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_compassmot_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_compassmot_status_t* compassmot_status) +{ + return mavlink_msg_compassmot_status_pack_chan(system_id, component_id, chan, msg, compassmot_status->throttle, compassmot_status->current, compassmot_status->interference, compassmot_status->CompensationX, compassmot_status->CompensationY, compassmot_status->CompensationZ); +} + +/** + * @brief Send a compassmot_status message + * @param chan MAVLink channel to send the message + * + * @param throttle throttle (percent*10) + * @param current current (amps) + * @param interference interference (percent) + * @param CompensationX Motor Compensation X + * @param CompensationY Motor Compensation Y + * @param CompensationZ Motor Compensation Z + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_compassmot_status_send(mavlink_channel_t chan, uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN]; + _mav_put_float(buf, 0, current); + _mav_put_float(buf, 4, CompensationX); + _mav_put_float(buf, 8, CompensationY); + _mav_put_float(buf, 12, CompensationZ); + _mav_put_uint16_t(buf, 16, throttle); + _mav_put_uint16_t(buf, 18, interference); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +#else + mavlink_compassmot_status_t packet; + packet.current = current; + packet.CompensationX = CompensationX; + packet.CompensationY = CompensationY; + packet.CompensationZ = CompensationZ; + packet.throttle = throttle; + packet.interference = interference; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_compassmot_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, current); + _mav_put_float(buf, 4, CompensationX); + _mav_put_float(buf, 8, CompensationY); + _mav_put_float(buf, 12, CompensationZ); + _mav_put_uint16_t(buf, 16, throttle); + _mav_put_uint16_t(buf, 18, interference); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +#else + mavlink_compassmot_status_t *packet = (mavlink_compassmot_status_t *)msgbuf; + packet->current = current; + packet->CompensationX = CompensationX; + packet->CompensationY = CompensationY; + packet->CompensationZ = CompensationZ; + packet->throttle = throttle; + packet->interference = interference; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE COMPASSMOT_STATUS UNPACKING + + +/** + * @brief Get field throttle from compassmot_status message + * + * @return throttle (percent*10) + */ +static inline uint16_t mavlink_msg_compassmot_status_get_throttle(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field current from compassmot_status message + * + * @return current (amps) + */ +static inline float mavlink_msg_compassmot_status_get_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field interference from compassmot_status message + * + * @return interference (percent) + */ +static inline uint16_t mavlink_msg_compassmot_status_get_interference(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field CompensationX from compassmot_status message + * + * @return Motor Compensation X + */ +static inline float mavlink_msg_compassmot_status_get_CompensationX(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field CompensationY from compassmot_status message + * + * @return Motor Compensation Y + */ +static inline float mavlink_msg_compassmot_status_get_CompensationY(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field CompensationZ from compassmot_status message + * + * @return Motor Compensation Z + */ +static inline float mavlink_msg_compassmot_status_get_CompensationZ(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Decode a compassmot_status message into a struct + * + * @param msg The message to decode + * @param compassmot_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_compassmot_status_decode(const mavlink_message_t* msg, mavlink_compassmot_status_t* compassmot_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + compassmot_status->current = mavlink_msg_compassmot_status_get_current(msg); + compassmot_status->CompensationX = mavlink_msg_compassmot_status_get_CompensationX(msg); + compassmot_status->CompensationY = mavlink_msg_compassmot_status_get_CompensationY(msg); + compassmot_status->CompensationZ = mavlink_msg_compassmot_status_get_CompensationZ(msg); + compassmot_status->throttle = mavlink_msg_compassmot_status_get_throttle(msg); + compassmot_status->interference = mavlink_msg_compassmot_status_get_interference(msg); +#else + memcpy(compassmot_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h index 9200eefa0..7bad1b23d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_data16_send(mavlink_channel_t chan, uint8_t type, #endif } +#if MAVLINK_MSG_ID_DATA16_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data16_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN); +#endif +#else + mavlink_data16_t *packet = (mavlink_data16_t *)msgbuf; + packet->type = type; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)packet, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)packet, MAVLINK_MSG_ID_DATA16_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA16 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h index 3afedb787..df7a2ec80 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_data32_send(mavlink_channel_t chan, uint8_t type, #endif } +#if MAVLINK_MSG_ID_DATA32_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data32_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN); +#endif +#else + mavlink_data32_t *packet = (mavlink_data32_t *)msgbuf; + packet->type = type; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)packet, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)packet, MAVLINK_MSG_ID_DATA32_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA32 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h index 6931ada16..c354f8908 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_data64_send(mavlink_channel_t chan, uint8_t type, #endif } +#if MAVLINK_MSG_ID_DATA64_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data64_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 64); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN); +#endif +#else + mavlink_data64_t *packet = (mavlink_data64_t *)msgbuf; + packet->type = type; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*64); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)packet, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)packet, MAVLINK_MSG_ID_DATA64_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA64 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h index cffc7d7e7..634e3f81d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_data96_send(mavlink_channel_t chan, uint8_t type, #endif } +#if MAVLINK_MSG_ID_DATA96_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data96_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 96); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN); +#endif +#else + mavlink_data96_t *packet = (mavlink_data96_t *)msgbuf; + packet->type = type; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*96); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)packet, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)packet, MAVLINK_MSG_ID_DATA96_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA96 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h index c6518c419..d5f9e7132 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_digicam_configure_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_digicam_configure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, extra_value); + _mav_put_uint16_t(buf, 4, shutter_speed); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + _mav_put_uint8_t(buf, 8, mode); + _mav_put_uint8_t(buf, 9, aperture); + _mav_put_uint8_t(buf, 10, iso); + _mav_put_uint8_t(buf, 11, exposure_type); + _mav_put_uint8_t(buf, 12, command_id); + _mav_put_uint8_t(buf, 13, engine_cut_off); + _mav_put_uint8_t(buf, 14, extra_param); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif +#else + mavlink_digicam_configure_t *packet = (mavlink_digicam_configure_t *)msgbuf; + packet->extra_value = extra_value; + packet->shutter_speed = shutter_speed; + packet->target_system = target_system; + packet->target_component = target_component; + packet->mode = mode; + packet->aperture = aperture; + packet->iso = iso; + packet->exposure_type = exposure_type; + packet->command_id = command_id; + packet->engine_cut_off = engine_cut_off; + packet->extra_param = extra_param; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DIGICAM_CONFIGURE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h index bfa5414a3..68484f7b8 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_digicam_control_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_digicam_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, extra_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, session); + _mav_put_uint8_t(buf, 7, zoom_pos); + _mav_put_int8_t(buf, 8, zoom_step); + _mav_put_uint8_t(buf, 9, focus_lock); + _mav_put_uint8_t(buf, 10, shot); + _mav_put_uint8_t(buf, 11, command_id); + _mav_put_uint8_t(buf, 12, extra_param); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif +#else + mavlink_digicam_control_t *packet = (mavlink_digicam_control_t *)msgbuf; + packet->extra_value = extra_value; + packet->target_system = target_system; + packet->target_component = target_component; + packet->session = session; + packet->zoom_pos = zoom_pos; + packet->zoom_step = zoom_step; + packet->focus_lock = focus_lock; + packet->shot = shot; + packet->command_id = command_id; + packet->extra_param = extra_param; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DIGICAM_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h index fe3677d53..4021c7a09 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_fence_fetch_point_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_fence_fetch_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif +#else + mavlink_fence_fetch_point_t *packet = (mavlink_fence_fetch_point_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->idx = idx; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FENCE_FETCH_POINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h index febda6cdc..6fc2c83fc 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_fence_point_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_FENCE_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_fence_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, lat); + _mav_put_float(buf, 4, lng); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t(buf, 10, idx); + _mav_put_uint8_t(buf, 11, count); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif +#else + mavlink_fence_point_t *packet = (mavlink_fence_point_t *)msgbuf; + packet->lat = lat; + packet->lng = lng; + packet->target_system = target_system; + packet->target_component = target_component; + packet->idx = idx; + packet->count = count; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FENCE_POINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h index 612090406..c50d37724 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_FENCE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, breach_time); + _mav_put_uint16_t(buf, 4, breach_count); + _mav_put_uint8_t(buf, 6, breach_status); + _mav_put_uint8_t(buf, 7, breach_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif +#else + mavlink_fence_status_t *packet = (mavlink_fence_status_t *)msgbuf; + packet->breach_time = breach_time; + packet->breach_count = breach_count; + packet->breach_status = breach_status; + packet->breach_type = breach_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FENCE_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h index 2f5dea513..acf031f62 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_hwstatus_send(mavlink_channel_t chan, uint16_t Vc #endif } +#if MAVLINK_MSG_ID_HWSTATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hwstatus_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint8_t(buf, 2, I2Cerr); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif +#else + mavlink_hwstatus_t *packet = (mavlink_hwstatus_t *)msgbuf; + packet->Vcc = Vcc; + packet->I2Cerr = I2Cerr; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)packet, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)packet, MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HWSTATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h index 34743fd02..5fef93718 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_limits_status_send(mavlink_channel_t chan, uint8_ #endif } +#if MAVLINK_MSG_ID_LIMITS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_limits_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, last_trigger); + _mav_put_uint32_t(buf, 4, last_action); + _mav_put_uint32_t(buf, 8, last_recovery); + _mav_put_uint32_t(buf, 12, last_clear); + _mav_put_uint16_t(buf, 16, breach_count); + _mav_put_uint8_t(buf, 18, limits_state); + _mav_put_uint8_t(buf, 19, mods_enabled); + _mav_put_uint8_t(buf, 20, mods_required); + _mav_put_uint8_t(buf, 21, mods_triggered); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif +#else + mavlink_limits_status_t *packet = (mavlink_limits_status_t *)msgbuf; + packet->last_trigger = last_trigger; + packet->last_action = last_action; + packet->last_recovery = last_recovery; + packet->last_clear = last_clear; + packet->breach_count = breach_count; + packet->limits_state = limits_state; + packet->mods_enabled = mods_enabled; + packet->mods_required = mods_required; + packet->mods_triggered = mods_triggered; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LIMITS_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h index 55f772bbc..c64b2e937 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_meminfo_send(mavlink_channel_t chan, uint16_t brk #endif } +#if MAVLINK_MSG_ID_MEMINFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_meminfo_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t brkval, uint16_t freemem) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, brkval); + _mav_put_uint16_t(buf, 2, freemem); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN); +#endif +#else + mavlink_meminfo_t *packet = (mavlink_meminfo_t *)msgbuf; + packet->brkval = brkval; + packet->freemem = freemem; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)packet, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)packet, MAVLINK_MSG_ID_MEMINFO_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MEMINFO UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h index de717dfa4..350bb0b78 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_mount_configure_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mount_configure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mount_mode); + _mav_put_uint8_t(buf, 3, stab_roll); + _mav_put_uint8_t(buf, 4, stab_pitch); + _mav_put_uint8_t(buf, 5, stab_yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif +#else + mavlink_mount_configure_t *packet = (mavlink_mount_configure_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->mount_mode = mount_mode; + packet->stab_roll = stab_roll; + packet->stab_pitch = stab_pitch; + packet->stab_yaw = stab_yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MOUNT_CONFIGURE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h index 44416353e..72d6e2419 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_mount_control_send(mavlink_channel_t chan, uint8_ #endif } +#if MAVLINK_MSG_ID_MOUNT_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mount_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, input_a); + _mav_put_int32_t(buf, 4, input_b); + _mav_put_int32_t(buf, 8, input_c); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + _mav_put_uint8_t(buf, 14, save_position); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif +#else + mavlink_mount_control_t *packet = (mavlink_mount_control_t *)msgbuf; + packet->input_a = input_a; + packet->input_b = input_b; + packet->input_c = input_c; + packet->target_system = target_system; + packet->target_component = target_component; + packet->save_position = save_position; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MOUNT_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h index 4905905dc..c140dd2c7 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_mount_status_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_MOUNT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mount_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, pointing_a); + _mav_put_int32_t(buf, 4, pointing_b); + _mav_put_int32_t(buf, 8, pointing_c); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif +#else + mavlink_mount_status_t *packet = (mavlink_mount_status_t *)msgbuf; + packet->pointing_a = pointing_a; + packet->pointing_b = pointing_b; + packet->pointing_c = pointing_c; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MOUNT_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h index 8e9740e82..830eb4639 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi, #endif } +#if MAVLINK_MSG_ID_RADIO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_radio_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN); +#endif +#else + mavlink_radio_t *packet = (mavlink_radio_t *)msgbuf; + packet->rxerrors = rxerrors; + packet->fixed = fixed; + packet->rssi = rssi; + packet->remrssi = remrssi; + packet->txbuf = txbuf; + packet->noise = noise; + packet->remnoise = remnoise; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)packet, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)packet, MAVLINK_MSG_ID_RADIO_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RADIO UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h index f057e3c33..4598251fe 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_rally_fetch_point_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rally_fetch_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +#else + mavlink_rally_fetch_point_t *packet = (mavlink_rally_fetch_point_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->idx = idx; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RALLY_FETCH_POINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h index 2c21db4c0..b44e764a9 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_rally_point_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_RALLY_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rally_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lng); + _mav_put_int16_t(buf, 8, alt); + _mav_put_int16_t(buf, 10, break_alt); + _mav_put_uint16_t(buf, 12, land_dir); + _mav_put_uint8_t(buf, 14, target_system); + _mav_put_uint8_t(buf, 15, target_component); + _mav_put_uint8_t(buf, 16, idx); + _mav_put_uint8_t(buf, 17, count); + _mav_put_uint8_t(buf, 18, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +#else + mavlink_rally_point_t *packet = (mavlink_rally_point_t *)msgbuf; + packet->lat = lat; + packet->lng = lng; + packet->alt = alt; + packet->break_alt = break_alt; + packet->land_dir = land_dir; + packet->target_system = target_system; + packet->target_component = target_component; + packet->idx = idx; + packet->count = count; + packet->flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RALLY_POINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h index c476447a8..464ce8a47 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_rangefinder_send(mavlink_channel_t chan, float di #endif } +#if MAVLINK_MSG_ID_RANGEFINDER_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rangefinder_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float distance, float voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, distance); + _mav_put_float(buf, 4, voltage); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif +#else + mavlink_rangefinder_t *packet = (mavlink_rangefinder_t *)msgbuf; + packet->distance = distance; + packet->voltage = voltage; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)packet, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RANGEFINDER UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h index 31b7d989d..d18f31c95 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h @@ -267,6 +267,60 @@ static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16 #endif } +#if MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sensor_offsets_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, mag_declination); + _mav_put_int32_t(buf, 4, raw_press); + _mav_put_int32_t(buf, 8, raw_temp); + _mav_put_float(buf, 12, gyro_cal_x); + _mav_put_float(buf, 16, gyro_cal_y); + _mav_put_float(buf, 20, gyro_cal_z); + _mav_put_float(buf, 24, accel_cal_x); + _mav_put_float(buf, 28, accel_cal_y); + _mav_put_float(buf, 32, accel_cal_z); + _mav_put_int16_t(buf, 36, mag_ofs_x); + _mav_put_int16_t(buf, 38, mag_ofs_y); + _mav_put_int16_t(buf, 40, mag_ofs_z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif +#else + mavlink_sensor_offsets_t *packet = (mavlink_sensor_offsets_t *)msgbuf; + packet->mag_declination = mag_declination; + packet->raw_press = raw_press; + packet->raw_temp = raw_temp; + packet->gyro_cal_x = gyro_cal_x; + packet->gyro_cal_y = gyro_cal_y; + packet->gyro_cal_z = gyro_cal_z; + packet->accel_cal_x = accel_cal_x; + packet->accel_cal_y = accel_cal_y; + packet->accel_cal_z = accel_cal_z; + packet->mag_ofs_x = mag_ofs_x; + packet->mag_ofs_y = mag_ofs_y; + packet->mag_ofs_z = mag_ofs_z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SENSOR_OFFSETS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h index b2c629c39..fc6aa71e0 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_mag_offsets_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, mag_ofs_x); + _mav_put_int16_t(buf, 2, mag_ofs_y); + _mav_put_int16_t(buf, 4, mag_ofs_z); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif +#else + mavlink_set_mag_offsets_t *packet = (mavlink_set_mag_offsets_t *)msgbuf; + packet->mag_ofs_x = mag_ofs_x; + packet->mag_ofs_y = mag_ofs_y; + packet->mag_ofs_z = mag_ofs_z; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_MAG_OFFSETS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h index 8ef44f76d..48cfb6ffd 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, #endif } +#if MAVLINK_MSG_ID_SIMSTATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_simstate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, xacc); + _mav_put_float(buf, 16, yacc); + _mav_put_float(buf, 20, zacc); + _mav_put_float(buf, 24, xgyro); + _mav_put_float(buf, 28, ygyro); + _mav_put_float(buf, 32, zgyro); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif +#else + mavlink_simstate_t *packet = (mavlink_simstate_t *)msgbuf; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->lat = lat; + packet->lng = lng; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)packet, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)packet, MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SIMSTATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h index 7608a7bd1..5d5edc4cc 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_wind_send(mavlink_channel_t chan, float direction #endif } +#if MAVLINK_MSG_ID_WIND_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_wind_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float direction, float speed, float speed_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, direction); + _mav_put_float(buf, 4, speed); + _mav_put_float(buf, 8, speed_z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN); +#endif +#else + mavlink_wind_t *packet = (mavlink_wind_t *)msgbuf; + packet->direction = direction; + packet->speed = speed; + packet->speed_z = speed_z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)packet, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)packet, MAVLINK_MSG_ID_WIND_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE WIND UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h index b2496334f..489553ea4 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h @@ -1398,6 +1398,59 @@ static void mavlink_test_rally_fetch_point(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_compassmot_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_compassmot_status_t packet_in = { + 17.0, + }45.0, + }73.0, + }101.0, + }18067, + }18171, + }; + mavlink_compassmot_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.current = packet_in.current; + packet1.CompensationX = packet_in.CompensationX; + packet1.CompensationY = packet_in.CompensationY; + packet1.CompensationZ = packet_in.CompensationZ; + packet1.throttle = packet_in.throttle; + packet1.interference = packet_in.interference; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_compassmot_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_compassmot_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_compassmot_status_pack(system_id, component_id, &msg , packet1.throttle , packet1.current , packet1.interference , packet1.CompensationX , packet1.CompensationY , packet1.CompensationZ ); + mavlink_msg_compassmot_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_compassmot_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.throttle , packet1.current , packet1.interference , packet1.CompensationX , packet1.CompensationY , packet1.CompensationZ ); + mavlink_msg_compassmot_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h index ed7c86bcb..e78d5687a 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h +++ b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h @@ -366,6 +366,78 @@ static inline void mavlink_msg_aq_telemetry_f_send(mavlink_channel_t chan, uint1 #endif } +#if MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_aq_telemetry_f_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Index, float value1, float value2, float value3, float value4, float value5, float value6, float value7, float value8, float value9, float value10, float value11, float value12, float value13, float value14, float value15, float value16, float value17, float value18, float value19, float value20) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, value1); + _mav_put_float(buf, 4, value2); + _mav_put_float(buf, 8, value3); + _mav_put_float(buf, 12, value4); + _mav_put_float(buf, 16, value5); + _mav_put_float(buf, 20, value6); + _mav_put_float(buf, 24, value7); + _mav_put_float(buf, 28, value8); + _mav_put_float(buf, 32, value9); + _mav_put_float(buf, 36, value10); + _mav_put_float(buf, 40, value11); + _mav_put_float(buf, 44, value12); + _mav_put_float(buf, 48, value13); + _mav_put_float(buf, 52, value14); + _mav_put_float(buf, 56, value15); + _mav_put_float(buf, 60, value16); + _mav_put_float(buf, 64, value17); + _mav_put_float(buf, 68, value18); + _mav_put_float(buf, 72, value19); + _mav_put_float(buf, 76, value20); + _mav_put_uint16_t(buf, 80, Index); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#endif +#else + mavlink_aq_telemetry_f_t *packet = (mavlink_aq_telemetry_f_t *)msgbuf; + packet->value1 = value1; + packet->value2 = value2; + packet->value3 = value3; + packet->value4 = value4; + packet->value5 = value5; + packet->value6 = value6; + packet->value7 = value7; + packet->value8 = value8; + packet->value9 = value9; + packet->value10 = value10; + packet->value11 = value11; + packet->value12 = value12; + packet->value13 = value13; + packet->value14 = value14; + packet->value15 = value15; + packet->value16 = value16; + packet->value17 = value17; + packet->value18 = value18; + packet->value19 = value19; + packet->value20 = value20; + packet->Index = Index; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AQ_TELEMETRY_F UNPACKING diff --git a/mavlink/include/mavlink/v1.0/autoquad/version.h b/mavlink/include/mavlink/v1.0/autoquad/version.h index d125e92b1..5d2f33b60 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/version.h +++ b/mavlink/include/mavlink/v1.0/autoquad/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Tue Feb 4 15:27:43 2014" +#define MAVLINK_BUILD_DATE "Sun Apr 13 09:38:19 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h index 7e1cf765b..020211d40 100644 --- a/mavlink/include/mavlink/v1.0/common/common.h +++ b/mavlink/include/mavlink/v1.0/common/common.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -79,7 +79,8 @@ enum MAV_TYPE MAV_TYPE_TRICOPTER=15, /* Octorotor | */ MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ MAV_TYPE_KITE=17, /* Flapping wing | */ - MAV_TYPE_ENUM_END=18, /* | */ + MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ + MAV_TYPE_ENUM_END=19, /* | */ }; #endif @@ -228,7 +229,8 @@ enum MAV_SYS_STATUS_SENSOR MAV_SYS_STATUS_SENSOR_3D_GYRO2=131072, /* 0x20000 2nd 3D gyro | */ MAV_SYS_STATUS_SENSOR_3D_ACCEL2=262144, /* 0x40000 2nd 3D accelerometer | */ MAV_SYS_STATUS_SENSOR_3D_MAG2=524288, /* 0x80000 2nd 3D magnetometer | */ - MAV_SYS_STATUS_SENSOR_ENUM_END=524289, /* | */ + MAV_SYS_STATUS_GEOFENCE=1048576, /* 0x100000 geofence | */ + MAV_SYS_STATUS_SENSOR_ENUM_END=1048577, /* | */ }; #endif @@ -242,7 +244,9 @@ enum MAV_FRAME MAV_FRAME_MISSION=2, /* NOT a coordinate frame, indicates a mission command. | */ MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. | */ MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-down (x: east, y: north, z: up) | */ - MAV_FRAME_ENUM_END=5, /* | */ + MAV_FRAME_GLOBAL_INT=5, /* Global coordinate frame with some fields as scaled integers, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL). Lat / Lon are scaled * 1E7 to avoid floating point accuracy limitations. | */ + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT=6, /* Global coordinate frame with some fields as scaled integers, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. Lat / Lon are scaled * 1E7 to avoid floating point accuracy limitations. | */ + MAV_FRAME_ENUM_END=7, /* | */ }; #endif @@ -261,6 +265,57 @@ enum MAVLINK_DATA_STREAM_TYPE }; #endif +/** @brief */ +#ifndef HAVE_ENUM_FENCE_ACTION +#define HAVE_ENUM_FENCE_ACTION +enum FENCE_ACTION +{ + FENCE_ACTION_NONE=0, /* Disable fenced mode | */ + FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */ + FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */ + FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */ + FENCE_ACTION_ENUM_END=4, /* | */ +}; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_FENCE_BREACH +#define HAVE_ENUM_FENCE_BREACH +enum FENCE_BREACH +{ + FENCE_BREACH_NONE=0, /* No last fence breach | */ + FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */ + FENCE_BREACH_MAXALT=2, /* Breached maximum altitude | */ + FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */ + FENCE_BREACH_ENUM_END=4, /* | */ +}; +#endif + +/** @brief Enumeration of possible mount operation modes */ +#ifndef HAVE_ENUM_MAV_MOUNT_MODE +#define HAVE_ENUM_MAV_MOUNT_MODE +enum MAV_MOUNT_MODE +{ + MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization | */ + MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | */ + MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ + MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ + MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ + MAV_MOUNT_MODE_ENUM_END=5, /* | */ +}; +#endif + +/** @brief Enumeration of distance sensor types */ +#ifndef HAVE_ENUM_MAV_DISTANCE_SENSOR +#define HAVE_ENUM_MAV_DISTANCE_SENSOR +enum MAV_DISTANCE_SENSOR +{ + MAV_DISTANCE_SENSOR_LASER=0, /* Laser altimeter, e.g. LightWare SF02/F or PulsedLight units | */ + MAV_DISTANCE_SENSOR_ULTRASOUND=1, /* Laser altimeter, e.g. MaxBotix units | */ + MAV_DISTANCE_SENSOR_ENUM_END=2, /* | */ +}; +#endif + /** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */ #ifndef HAVE_ENUM_MAV_CMD #define HAVE_ENUM_MAV_CMD @@ -275,6 +330,7 @@ enum MAV_CMD MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ @@ -292,8 +348,16 @@ enum MAV_CMD MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ @@ -433,6 +497,48 @@ enum MAV_SEVERITY }; #endif +/** @brief Power supply status flags (bitmask) */ +#ifndef HAVE_ENUM_MAV_POWER_STATUS +#define HAVE_ENUM_MAV_POWER_STATUS +enum MAV_POWER_STATUS +{ + MAV_POWER_STATUS_BRICK_VALID=1, /* main brick power supply valid | */ + MAV_POWER_STATUS_SERVO_VALID=2, /* main servo power supply valid for FMU | */ + MAV_POWER_STATUS_USB_CONNECTED=4, /* USB power is connected | */ + MAV_POWER_STATUS_PERIPH_OVERCURRENT=8, /* peripheral supply is in over-current state | */ + MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT=16, /* hi-power peripheral supply is in over-current state | */ + MAV_POWER_STATUS_CHANGED=32, /* Power status has changed since boot | */ + MAV_POWER_STATUS_ENUM_END=33, /* | */ +}; +#endif + +/** @brief SERIAL_CONTROL device types */ +#ifndef HAVE_ENUM_SERIAL_CONTROL_DEV +#define HAVE_ENUM_SERIAL_CONTROL_DEV +enum SERIAL_CONTROL_DEV +{ + SERIAL_CONTROL_DEV_TELEM1=0, /* First telemetry port | */ + SERIAL_CONTROL_DEV_TELEM2=1, /* Second telemetry port | */ + SERIAL_CONTROL_DEV_GPS1=2, /* First GPS port | */ + SERIAL_CONTROL_DEV_GPS2=3, /* Second GPS port | */ + SERIAL_CONTROL_DEV_ENUM_END=4, /* | */ +}; +#endif + +/** @brief SERIAL_CONTROL flags (bitmask) */ +#ifndef HAVE_ENUM_SERIAL_CONTROL_FLAG +#define HAVE_ENUM_SERIAL_CONTROL_FLAG +enum SERIAL_CONTROL_FLAG +{ + SERIAL_CONTROL_FLAG_REPLY=1, /* Set if this is a reply | */ + SERIAL_CONTROL_FLAG_RESPOND=2, /* Set if the sender wants the receiver to send a response as another SERIAL_CONTROL message | */ + SERIAL_CONTROL_FLAG_EXCLUSIVE=4, /* Set if access to the serial port should be removed from whatever driver is currently using it, giving exclusive access to the SERIAL_CONTROL protocol. The port can be handed back by sending a request without this flag set | */ + SERIAL_CONTROL_FLAG_BLOCKING=8, /* Block on writes to the serial port | */ + SERIAL_CONTROL_FLAG_MULTI=16, /* Send multiple replies until port is drained | */ + SERIAL_CONTROL_FLAG_ENUM_END=17, /* | */ +}; +#endif + // MAVLINK VERSION @@ -500,6 +606,7 @@ enum MAV_SEVERITY #include "./mavlink_msg_nav_controller_output.h" #include "./mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h" #include "./mavlink_msg_state_correction.h" +#include "./mavlink_msg_rc_channels.h" #include "./mavlink_msg_request_data_stream.h" #include "./mavlink_msg_data_stream.h" #include "./mavlink_msg_manual_control.h" @@ -538,8 +645,11 @@ enum MAV_SEVERITY #include "./mavlink_msg_log_request_end.h" #include "./mavlink_msg_gps_inject_data.h" #include "./mavlink_msg_gps2_raw.h" +#include "./mavlink_msg_power_status.h" +#include "./mavlink_msg_serial_control.h" #include "./mavlink_msg_data_transmission_handshake.h" #include "./mavlink_msg_encapsulated_data.h" +#include "./mavlink_msg_distance_sensor.h" #include "./mavlink_msg_battery_status.h" #include "./mavlink_msg_setpoint_8dof.h" #include "./mavlink_msg_setpoint_6dof.h" diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h index 8ddf5bf09..ff2f104d4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t ti #endif } +#if MAVLINK_MSG_ID_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, rollspeed); + _mav_put_float(buf, 20, pitchspeed); + _mav_put_float(buf, 24, yawspeed); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +#else + mavlink_attitude_t *packet = (mavlink_attitude_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ATTITUDE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h index 9f8d58759..3b97b40d0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h @@ -223,6 +223,52 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, q1); + _mav_put_float(buf, 8, q2); + _mav_put_float(buf, 12, q3); + _mav_put_float(buf, 16, q4); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +#else + mavlink_attitude_quaternion_t *packet = (mavlink_attitude_quaternion_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->q1 = q1; + packet->q2 = q2; + packet->q3 = q3; + packet->q4 = q4; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ATTITUDE_QUATERNION UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h index 5703a5987..f31b6bbf4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h @@ -146,6 +146,38 @@ static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char #endif } +#if MAVLINK_MSG_ID_AUTH_KEY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_auth_key_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *key) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_char_array(buf, 0, key, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +#else + mavlink_auth_key_t *packet = (mavlink_auth_key_t *)msgbuf; + + mav_array_memcpy(packet->key, key, sizeof(char)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AUTH_KEY UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h index 03e4d569e..faaabf9f7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 #endif } +#if MAVLINK_MSG_ID_BATTERY_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, current_consumed); + _mav_put_int32_t(buf, 4, energy_consumed); + _mav_put_uint16_t(buf, 8, voltage_cell_1); + _mav_put_uint16_t(buf, 10, voltage_cell_2); + _mav_put_uint16_t(buf, 12, voltage_cell_3); + _mav_put_uint16_t(buf, 14, voltage_cell_4); + _mav_put_uint16_t(buf, 16, voltage_cell_5); + _mav_put_uint16_t(buf, 18, voltage_cell_6); + _mav_put_int16_t(buf, 20, current_battery); + _mav_put_uint8_t(buf, 22, accu_id); + _mav_put_int8_t(buf, 23, battery_remaining); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +#else + mavlink_battery_status_t *packet = (mavlink_battery_status_t *)msgbuf; + packet->current_consumed = current_consumed; + packet->energy_consumed = energy_consumed; + packet->voltage_cell_1 = voltage_cell_1; + packet->voltage_cell_2 = voltage_cell_2; + packet->voltage_cell_3 = voltage_cell_3; + packet->voltage_cell_4 = voltage_cell_4; + packet->voltage_cell_5 = voltage_cell_5; + packet->voltage_cell_6 = voltage_cell_6; + packet->current_battery = current_battery; + packet->accu_id = accu_id; + packet->battery_remaining = battery_remaining; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE BATTERY_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h index 0b6de930d..3f0987f10 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_change_operator_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +#else + mavlink_change_operator_control_t *packet = (mavlink_change_operator_control_t *)msgbuf; + packet->target_system = target_system; + packet->control_request = control_request; + packet->version = version; + mav_array_memcpy(packet->passkey, passkey, sizeof(char)*25); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE CHANGE_OPERATOR_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h index c6f6a28e4..768e7ed5c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_ #endif } +#if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_change_operator_control_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +#else + mavlink_change_operator_control_ack_t *packet = (mavlink_change_operator_control_ack_t *)msgbuf; + packet->gcs_system_id = gcs_system_id; + packet->control_request = control_request; + packet->ack = ack; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE CHANGE_OPERATOR_CONTROL_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h index dca2fe681..d3d163041 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t #endif } +#if MAVLINK_MSG_ID_COMMAND_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t command, uint8_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, command); + _mav_put_uint8_t(buf, 2, result); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +#else + mavlink_command_ack_t *packet = (mavlink_command_ack_t *)msgbuf; + packet->command = command; + packet->result = result; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE COMMAND_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h index 8f705c0dd..161896b97 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_COMMAND_LONG_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_command_long_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, param5); + _mav_put_float(buf, 20, param6); + _mav_put_float(buf, 24, param7); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, confirmation); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +#else + mavlink_command_long_t *packet = (mavlink_command_long_t *)msgbuf; + packet->param1 = param1; + packet->param2 = param2; + packet->param3 = param3; + packet->param4 = param4; + packet->param5 = param5; + packet->param6 = param6; + packet->param7 = param7; + packet->command = command; + packet->target_system = target_system; + packet->target_component = target_component; + packet->confirmation = confirmation; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE COMMAND_LONG UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h index dc0768e12..640ffebf4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_data_stream_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data_stream_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint16_t message_rate, uint8_t on_off) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, message_rate); + _mav_put_uint8_t(buf, 2, stream_id); + _mav_put_uint8_t(buf, 3, on_off); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +#else + mavlink_data_stream_t *packet = (mavlink_data_stream_t *)msgbuf; + packet->message_rate = message_rate; + packet->stream_id = stream_id; + packet->on_off = on_off; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA_STREAM UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h index fdd8fddd8..d84a73709 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_ #endif } +#if MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data_transmission_handshake_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, size); + _mav_put_uint16_t(buf, 4, width); + _mav_put_uint16_t(buf, 6, height); + _mav_put_uint16_t(buf, 8, packets); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, payload); + _mav_put_uint8_t(buf, 12, jpg_quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +#else + mavlink_data_transmission_handshake_t *packet = (mavlink_data_transmission_handshake_t *)msgbuf; + packet->size = size; + packet->width = width; + packet->height = height; + packet->packets = packets; + packet->type = type; + packet->payload = payload; + packet->jpg_quality = jpg_quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA_TRANSMISSION_HANDSHAKE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h index 9a6ed87ee..2102af862 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint32_t time_ #endif } +#if MAVLINK_MSG_ID_DEBUG_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_debug_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t ind, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_uint8_t(buf, 8, ind); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +#else + mavlink_debug_t *packet = (mavlink_debug_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->value = value; + packet->ind = ind; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DEBUG UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h index 6cfc75212..67c339e89 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h @@ -184,6 +184,44 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const cha #endif } +#if MAVLINK_MSG_ID_DEBUG_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_debug_vect_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *name, uint64_t time_usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_char_array(buf, 20, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +#else + mavlink_debug_vect_t *packet = (mavlink_debug_vect_t *)msgbuf; + packet->time_usec = time_usec; + packet->x = x; + packet->y = y; + packet->z = z; + mav_array_memcpy(packet->name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DEBUG_VECT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h new file mode 100644 index 000000000..39b384396 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h @@ -0,0 +1,377 @@ +// MESSAGE DISTANCE_SENSOR PACKING + +#define MAVLINK_MSG_ID_DISTANCE_SENSOR 132 + +typedef struct __mavlink_distance_sensor_t +{ + uint32_t time_boot_ms; ///< Time since system boot + uint16_t min_distance; ///< Minimum distance the sensor can measure in centimeters + uint16_t max_distance; ///< Maximum distance the sensor can measure in centimeters + uint16_t current_distance; ///< Current distance reading + uint8_t type; ///< Type from MAV_DISTANCE_SENSOR enum. + uint8_t id; ///< Onboard ID of the sensor + uint8_t orientation; ///< Direction the sensor faces from FIXME enum. + uint8_t covariance; ///< Measurement covariance in centimeters, 0 for unknown / invalid readings +} mavlink_distance_sensor_t; + +#define MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN 14 +#define MAVLINK_MSG_ID_132_LEN 14 + +#define MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC 85 +#define MAVLINK_MSG_ID_132_CRC 85 + + + +#define MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR { \ + "DISTANCE_SENSOR", \ + 8, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_distance_sensor_t, time_boot_ms) }, \ + { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_distance_sensor_t, min_distance) }, \ + { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_distance_sensor_t, max_distance) }, \ + { "current_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_distance_sensor_t, current_distance) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_distance_sensor_t, type) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_distance_sensor_t, id) }, \ + { "orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_distance_sensor_t, orientation) }, \ + { "covariance", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_distance_sensor_t, covariance) }, \ + } \ +} + + +/** + * @brief Pack a distance_sensor message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Time since system boot + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from FIXME enum. + * @param min_distance Minimum distance the sensor can measure in centimeters + * @param max_distance Maximum distance the sensor can measure in centimeters + * @param current_distance Current distance reading + * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} + +/** + * @brief Pack a distance_sensor message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Time since system boot + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from FIXME enum. + * @param min_distance Minimum distance the sensor can measure in centimeters + * @param max_distance Maximum distance the sensor can measure in centimeters + * @param current_distance Current distance reading + * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t type,uint8_t id,uint8_t orientation,uint16_t min_distance,uint16_t max_distance,uint16_t current_distance,uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} + +/** + * @brief Encode a distance_sensor struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param distance_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_distance_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) +{ + return mavlink_msg_distance_sensor_pack(system_id, component_id, msg, distance_sensor->time_boot_ms, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->covariance); +} + +/** + * @brief Encode a distance_sensor struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param distance_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) +{ + return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->covariance); +} + +/** + * @brief Send a distance_sensor message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Time since system boot + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from FIXME enum. + * @param min_distance Minimum distance the sensor can measure in centimeters + * @param max_distance Maximum distance the sensor can measure in centimeters + * @param current_distance Current distance reading + * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#else + mavlink_distance_sensor_t *packet = (mavlink_distance_sensor_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->min_distance = min_distance; + packet->max_distance = max_distance; + packet->current_distance = current_distance; + packet->type = type; + packet->id = id; + packet->orientation = orientation; + packet->covariance = covariance; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DISTANCE_SENSOR UNPACKING + + +/** + * @brief Get field time_boot_ms from distance_sensor message + * + * @return Time since system boot + */ +static inline uint32_t mavlink_msg_distance_sensor_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field type from distance_sensor message + * + * @return Type from MAV_DISTANCE_SENSOR enum. + */ +static inline uint8_t mavlink_msg_distance_sensor_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field id from distance_sensor message + * + * @return Onboard ID of the sensor + */ +static inline uint8_t mavlink_msg_distance_sensor_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field orientation from distance_sensor message + * + * @return Direction the sensor faces from FIXME enum. + */ +static inline uint8_t mavlink_msg_distance_sensor_get_orientation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field min_distance from distance_sensor message + * + * @return Minimum distance the sensor can measure in centimeters + */ +static inline uint16_t mavlink_msg_distance_sensor_get_min_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field max_distance from distance_sensor message + * + * @return Maximum distance the sensor can measure in centimeters + */ +static inline uint16_t mavlink_msg_distance_sensor_get_max_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field current_distance from distance_sensor message + * + * @return Current distance reading + */ +static inline uint16_t mavlink_msg_distance_sensor_get_current_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field covariance from distance_sensor message + * + * @return Measurement covariance in centimeters, 0 for unknown / invalid readings + */ +static inline uint8_t mavlink_msg_distance_sensor_get_covariance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Decode a distance_sensor message into a struct + * + * @param msg The message to decode + * @param distance_sensor C-struct to decode the message contents into + */ +static inline void mavlink_msg_distance_sensor_decode(const mavlink_message_t* msg, mavlink_distance_sensor_t* distance_sensor) +{ +#if MAVLINK_NEED_BYTE_SWAP + distance_sensor->time_boot_ms = mavlink_msg_distance_sensor_get_time_boot_ms(msg); + distance_sensor->min_distance = mavlink_msg_distance_sensor_get_min_distance(msg); + distance_sensor->max_distance = mavlink_msg_distance_sensor_get_max_distance(msg); + distance_sensor->current_distance = mavlink_msg_distance_sensor_get_current_distance(msg); + distance_sensor->type = mavlink_msg_distance_sensor_get_type(msg); + distance_sensor->id = mavlink_msg_distance_sensor_get_id(msg); + distance_sensor->orientation = mavlink_msg_distance_sensor_get_orientation(msg); + distance_sensor->covariance = mavlink_msg_distance_sensor_get_covariance(msg); +#else + memcpy(distance_sensor, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h index 5900ea838..dacd7c9f4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h @@ -151,6 +151,38 @@ static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_encapsulated_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seqnr, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seqnr); + _mav_put_uint8_t_array(buf, 2, data, 253); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +#else + mavlink_encapsulated_data_t *packet = (mavlink_encapsulated_data_t *)msgbuf; + packet->seqnr = seqnr; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*253); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ENCAPSULATED_DATA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h index 4f31698d5..81c7031ce 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_file_transfer_dir_list_send(mavlink_channel_t cha #endif } +#if MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_file_transfer_dir_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t transfer_uid, const char *dir_path, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, transfer_uid); + _mav_put_uint8_t(buf, 248, flags); + _mav_put_char_array(buf, 8, dir_path, 240); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); +#endif +#else + mavlink_file_transfer_dir_list_t *packet = (mavlink_file_transfer_dir_list_t *)msgbuf; + packet->transfer_uid = transfer_uid; + packet->flags = flags; + mav_array_memcpy(packet->dir_path, dir_path, sizeof(char)*240); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FILE_TRANSFER_DIR_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h index fc6247fac..04981fa85 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_file_transfer_res_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_file_transfer_res_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t transfer_uid, uint8_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, transfer_uid); + _mav_put_uint8_t(buf, 8, result); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); +#endif +#else + mavlink_file_transfer_res_t *packet = (mavlink_file_transfer_res_t *)msgbuf; + packet->transfer_uid = transfer_uid; + packet->result = result; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FILE_TRANSFER_RES UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h index 05be77339..7b1fba519 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h @@ -184,6 +184,44 @@ static inline void mavlink_msg_file_transfer_start_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_file_transfer_start_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t transfer_uid, const char *dest_path, uint8_t direction, uint32_t file_size, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, transfer_uid); + _mav_put_uint32_t(buf, 8, file_size); + _mav_put_uint8_t(buf, 252, direction); + _mav_put_uint8_t(buf, 253, flags); + _mav_put_char_array(buf, 12, dest_path, 240); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); +#endif +#else + mavlink_file_transfer_start_t *packet = (mavlink_file_transfer_start_t *)msgbuf; + packet->transfer_uid = transfer_uid; + packet->file_size = file_size; + packet->direction = direction; + packet->flags = flags; + mav_array_memcpy(packet->dest_path, dest_path, sizeof(char)*240); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FILE_TRANSFER_START UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h index 7ed3d2a63..ba15bf365 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_position_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_int32_t(buf, 12, alt); + _mav_put_int32_t(buf, 16, relative_alt); + _mav_put_int16_t(buf, 20, vx); + _mav_put_int16_t(buf, 22, vy); + _mav_put_int16_t(buf, 24, vz); + _mav_put_uint16_t(buf, 26, hdg); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +#else + mavlink_global_position_int_t *packet = (mavlink_global_position_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->relative_alt = relative_alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->hdg = hdg; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GLOBAL_POSITION_INT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h index 1a1c97199..77d735cf4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_global_position_setpoint_int_send(mavlink_channel #endif } +#if MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_position_setpoint_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_int16_t(buf, 12, yaw); + _mav_put_uint8_t(buf, 14, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); +#endif +#else + mavlink_global_position_setpoint_int_t *packet = (mavlink_global_position_setpoint_int_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude = altitude; + packet->yaw = yaw; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GLOBAL_POSITION_SETPOINT_INT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h index f7be74c91..b2b75d7ef 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan #endif } +#if MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_global_vision_position_estimate_t *packet = (mavlink_global_vision_position_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GLOBAL_VISION_POSITION_ESTIMATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h index 17e5bd002..b184e7c9c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h @@ -267,6 +267,60 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti #endif } +#if MAVLINK_MSG_ID_GPS2_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#else + mavlink_gps2_raw_t *packet = (mavlink_gps2_raw_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->dgps_age = dgps_age; + packet->eph = eph; + packet->epv = epv; + packet->vel = vel; + packet->cog = cog; + packet->fix_type = fix_type; + packet->satellites_visible = satellites_visible; + packet->dgps_numch = dgps_numch; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS2_RAW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h index 016e9cb0e..1589c8ca5 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, in #endif } +#if MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +#else + mavlink_gps_global_origin_t *packet = (mavlink_gps_global_origin_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude = altitude; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS_GLOBAL_ORIGIN UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h index 485d8a4af..362e2d7b9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_gps_inject_data_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_inject_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#else + mavlink_gps_inject_data_t *packet = (mavlink_gps_inject_data_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS_INJECT_DATA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h index a105f8cda..b53996206 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t #endif } +#if MAVLINK_MSG_ID_GPS_RAW_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_uint16_t(buf, 26, cog); + _mav_put_uint8_t(buf, 28, fix_type); + _mav_put_uint8_t(buf, 29, satellites_visible); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +#else + mavlink_gps_raw_int_t *packet = (mavlink_gps_raw_int_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->eph = eph; + packet->epv = epv; + packet->vel = vel; + packet->cog = cog; + packet->fix_type = fix_type; + packet->satellites_visible = satellites_visible; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS_RAW_INT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h index 28d6b57d1..10659d0dd 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h @@ -199,6 +199,46 @@ static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t s #endif } +#if MAVLINK_MSG_ID_GPS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t *satellite_prn, const uint8_t *satellite_used, const uint8_t *satellite_elevation, const uint8_t *satellite_azimuth, const uint8_t *satellite_snr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); + _mav_put_uint8_t_array(buf, 21, satellite_used, 20); + _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +#else + mavlink_gps_status_t *packet = (mavlink_gps_status_t *)msgbuf; + packet->satellites_visible = satellites_visible; + mav_array_memcpy(packet->satellite_prn, satellite_prn, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_used, satellite_used, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_snr, satellite_snr, sizeof(uint8_t)*20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h index 826138fad..902e381ca 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h @@ -198,6 +198,48 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty #endif } +#if MAVLINK_MSG_ID_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, autopilot); + _mav_put_uint8_t(buf, 6, base_mode); + _mav_put_uint8_t(buf, 7, system_status); + _mav_put_uint8_t(buf, 8, 3); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +#else + mavlink_heartbeat_t *packet = (mavlink_heartbeat_t *)msgbuf; + packet->custom_mode = custom_mode; + packet->type = type; + packet->autopilot = autopilot; + packet->base_mode = base_mode; + packet->system_status = system_status; + packet->mavlink_version = 3; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HEARTBEAT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h index 0dcd95ed3..2749cb097 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h @@ -300,6 +300,66 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t #endif } +#if MAVLINK_MSG_ID_HIGHRES_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint16_t(buf, 60, fields_updated); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +#else + mavlink_highres_imu_t *packet = (mavlink_highres_imu_t *)msgbuf; + packet->time_usec = time_usec; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + packet->abs_pressure = abs_pressure; + packet->diff_pressure = diff_pressure; + packet->pressure_alt = pressure_alt; + packet->temperature = temperature; + packet->fields_updated = fields_updated; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIGHRES_IMU UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h index aed5108d0..f7507b1cf 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_ #endif } +#if MAVLINK_MSG_ID_HIL_CONTROLS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_controls_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_float(buf, 24, aux1); + _mav_put_float(buf, 28, aux2); + _mav_put_float(buf, 32, aux3); + _mav_put_float(buf, 36, aux4); + _mav_put_uint8_t(buf, 40, mode); + _mav_put_uint8_t(buf, 41, nav_mode); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +#else + mavlink_hil_controls_t *packet = (mavlink_hil_controls_t *)msgbuf; + packet->time_usec = time_usec; + packet->roll_ailerons = roll_ailerons; + packet->pitch_elevator = pitch_elevator; + packet->yaw_rudder = yaw_rudder; + packet->throttle = throttle; + packet->aux1 = aux1; + packet->aux2 = aux2; + packet->aux3 = aux3; + packet->aux4 = aux4; + packet->mode = mode; + packet->nav_mode = nav_mode; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_CONTROLS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h index 91aec5b08..a22c0472f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h @@ -278,6 +278,62 @@ static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t tim #endif } +#if MAVLINK_MSG_ID_HIL_GPS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_int16_t(buf, 26, vn); + _mav_put_int16_t(buf, 28, ve); + _mav_put_int16_t(buf, 30, vd); + _mav_put_uint16_t(buf, 32, cog); + _mav_put_uint8_t(buf, 34, fix_type); + _mav_put_uint8_t(buf, 35, satellites_visible); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +#else + mavlink_hil_gps_t *packet = (mavlink_hil_gps_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->eph = eph; + packet->epv = epv; + packet->vel = vel; + packet->vn = vn; + packet->ve = ve; + packet->vd = vd; + packet->cog = cog; + packet->fix_type = fix_type; + packet->satellites_visible = satellites_visible; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_GPS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h index acb1392e1..eaadf2435 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h @@ -223,6 +223,52 @@ static inline void mavlink_msg_hil_optical_flow_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +#else + mavlink_hil_optical_flow_t *packet = (mavlink_hil_optical_flow_t *)msgbuf; + packet->time_usec = time_usec; + packet->flow_comp_m_x = flow_comp_m_x; + packet->flow_comp_m_y = flow_comp_m_y; + packet->ground_distance = ground_distance; + packet->flow_x = flow_x; + packet->flow_y = flow_y; + packet->sensor_id = sensor_id; + packet->quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_OPTICAL_FLOW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h index a42bde50b..227cd9d94 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h @@ -289,6 +289,64 @@ static inline void mavlink_msg_hil_rc_inputs_raw_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_rc_inputs_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, chan1_raw); + _mav_put_uint16_t(buf, 10, chan2_raw); + _mav_put_uint16_t(buf, 12, chan3_raw); + _mav_put_uint16_t(buf, 14, chan4_raw); + _mav_put_uint16_t(buf, 16, chan5_raw); + _mav_put_uint16_t(buf, 18, chan6_raw); + _mav_put_uint16_t(buf, 20, chan7_raw); + _mav_put_uint16_t(buf, 22, chan8_raw); + _mav_put_uint16_t(buf, 24, chan9_raw); + _mav_put_uint16_t(buf, 26, chan10_raw); + _mav_put_uint16_t(buf, 28, chan11_raw); + _mav_put_uint16_t(buf, 30, chan12_raw); + _mav_put_uint8_t(buf, 32, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +#else + mavlink_hil_rc_inputs_raw_t *packet = (mavlink_hil_rc_inputs_raw_t *)msgbuf; + packet->time_usec = time_usec; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + packet->chan9_raw = chan9_raw; + packet->chan10_raw = chan10_raw; + packet->chan11_raw = chan11_raw; + packet->chan12_raw = chan12_raw; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_RC_INPUTS_RAW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h index 6c2667473..8672f8b8a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h @@ -300,6 +300,66 @@ static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t #endif } +#if MAVLINK_MSG_ID_HIL_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint32_t(buf, 60, fields_updated); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +#else + mavlink_hil_sensor_t *packet = (mavlink_hil_sensor_t *)msgbuf; + packet->time_usec = time_usec; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + packet->abs_pressure = abs_pressure; + packet->diff_pressure = diff_pressure; + packet->pressure_alt = pressure_alt; + packet->temperature = temperature; + packet->fields_updated = fields_updated; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_SENSOR UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h index bcc857767..923ed60b9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h @@ -311,6 +311,68 @@ static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t t #endif } +#if MAVLINK_MSG_ID_HIL_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +#else + mavlink_hil_state_t *packet = (mavlink_hil_state_t *)msgbuf; + packet->time_usec = time_usec; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_STATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h index 732176193..1a028bdf9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h @@ -305,6 +305,66 @@ static inline void mavlink_msg_hil_state_quaternion_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_state_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, rollspeed); + _mav_put_float(buf, 28, pitchspeed); + _mav_put_float(buf, 32, yawspeed); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lon); + _mav_put_int32_t(buf, 44, alt); + _mav_put_int16_t(buf, 48, vx); + _mav_put_int16_t(buf, 50, vy); + _mav_put_int16_t(buf, 52, vz); + _mav_put_uint16_t(buf, 54, ind_airspeed); + _mav_put_uint16_t(buf, 56, true_airspeed); + _mav_put_int16_t(buf, 58, xacc); + _mav_put_int16_t(buf, 60, yacc); + _mav_put_int16_t(buf, 62, zacc); + _mav_put_float_array(buf, 8, attitude_quaternion, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +#else + mavlink_hil_state_quaternion_t *packet = (mavlink_hil_state_quaternion_t *)msgbuf; + packet->time_usec = time_usec; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->ind_airspeed = ind_airspeed; + packet->true_airspeed = true_airspeed; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + mav_array_memcpy(packet->attitude_quaternion, attitude_quaternion, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_STATE_QUATERNION UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h index a0b72c0e1..e18a94869 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_local_position_ned_send(mavlink_channel_t chan, u #endif } +#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_position_ned_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +#else + mavlink_local_position_ned_t *packet = (mavlink_local_position_ned_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOCAL_POSITION_NED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h index 8c4686202..af7d195b0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_local_position_ned_system_global_offset_send(mavl #endif } +#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_position_ned_system_global_offset_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +#else + mavlink_local_position_ned_system_global_offset_t *packet = (mavlink_local_position_ned_system_global_offset_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h index 1794815f8..8dae721be 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_position_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t coordinate_frame, float x, float y, float z, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint8_t(buf, 16, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); +#endif +#else + mavlink_local_position_setpoint_t *packet = (mavlink_local_position_setpoint_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->yaw = yaw; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOCAL_POSITION_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h index 1cf5d15e4..48641f3eb 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_log_data_send(mavlink_channel_t chan, uint16_t id #endif } +#if MAVLINK_MSG_ID_LOG_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint16_t(buf, 4, id); + _mav_put_uint8_t(buf, 6, count); + _mav_put_uint8_t_array(buf, 7, data, 90); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +#else + mavlink_log_data_t *packet = (mavlink_log_data_t *)msgbuf; + packet->ofs = ofs; + packet->id = id; + packet->count = count; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*90); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_DATA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h index 681d8f07c..8ecaec3ae 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_log_entry_send(mavlink_channel_t chan, uint16_t i #endif } +#if MAVLINK_MSG_ID_LOG_ENTRY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_entry_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 4, size); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint16_t(buf, 10, num_logs); + _mav_put_uint16_t(buf, 12, last_log_num); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +#else + mavlink_log_entry_t *packet = (mavlink_log_entry_t *)msgbuf; + packet->time_utc = time_utc; + packet->size = size; + packet->id = id; + packet->num_logs = num_logs; + packet->last_log_num = last_log_num; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_ENTRY UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h index feafeaf16..957c4d4cc 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_log_erase_send(mavlink_channel_t chan, uint8_t ta #endif } +#if MAVLINK_MSG_ID_LOG_ERASE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_erase_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +#else + mavlink_log_erase_t *packet = (mavlink_log_erase_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_ERASE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h index 5be9eea47..ef5cbb67c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_log_request_data_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_request_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint32_t(buf, 4, count); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint8_t(buf, 10, target_system); + _mav_put_uint8_t(buf, 11, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +#else + mavlink_log_request_data_t *packet = (mavlink_log_request_data_t *)msgbuf; + packet->ofs = ofs; + packet->count = count; + packet->id = id; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_REQUEST_DATA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h index 48a5a03b4..23fcca29f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_log_request_end_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_LOG_REQUEST_END_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_request_end_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +#else + mavlink_log_request_end_t *packet = (mavlink_log_request_end_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_REQUEST_END UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h index 7a5b25c17..e511b5312 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_log_request_list_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, start); + _mav_put_uint16_t(buf, 2, end); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +#else + mavlink_log_request_list_t *packet = (mavlink_log_request_list_t *)msgbuf; + packet->start = start; + packet->end = end; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_REQUEST_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h index 6b6e9e148..e93b759d0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8 #endif } +#if MAVLINK_MSG_ID_MANUAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_manual_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, x); + _mav_put_int16_t(buf, 2, y); + _mav_put_int16_t(buf, 4, z); + _mav_put_int16_t(buf, 6, r); + _mav_put_uint16_t(buf, 8, buttons); + _mav_put_uint8_t(buf, 10, target); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +#else + mavlink_manual_control_t *packet = (mavlink_manual_control_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->r = r; + packet->buttons = buttons; + packet->target = target; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MANUAL_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h index a694947c1..b27626726 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_manual_setpoint_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_manual_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust, uint8_t mode_switch, uint8_t manual_override_switch) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + _mav_put_uint8_t(buf, 20, mode_switch); + _mav_put_uint8_t(buf, 21, manual_override_switch); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +#else + mavlink_manual_setpoint_t *packet = (mavlink_manual_setpoint_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->thrust = thrust; + packet->mode_switch = mode_switch; + packet->manual_override_switch = manual_override_switch; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MANUAL_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h index 5f79329c2..2eb60cc0e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t #endif } +#if MAVLINK_MSG_ID_MEMORY_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_memory_vect_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t address, uint8_t ver, uint8_t type, const int8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, address); + _mav_put_uint8_t(buf, 2, ver); + _mav_put_uint8_t(buf, 3, type); + _mav_put_int8_t_array(buf, 4, value, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +#else + mavlink_memory_vect_t *packet = (mavlink_memory_vect_t *)msgbuf; + packet->address = address; + packet->ver = ver; + packet->type = type; + mav_array_memcpy(packet->value, value, sizeof(int8_t)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MEMORY_VECT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h index 7421d8394..43afd00f7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_MISSION_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +#else + mavlink_mission_ack_t *packet = (mavlink_mission_ack_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->type = type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h index 8f441c8e5..120986873 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_clear_all_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +#else + mavlink_mission_clear_all_t *packet = (mavlink_mission_clear_all_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_CLEAR_ALL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h index eac779306..7e4748eae 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_ #endif } +#if MAVLINK_MSG_ID_MISSION_COUNT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, count); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +#else + mavlink_mission_count_t *packet = (mavlink_mission_count_t *)msgbuf; + packet->count = count; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_COUNT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h index dbcdbd3f9..201b7a6fb 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h @@ -146,6 +146,38 @@ static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_MISSION_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +#else + mavlink_mission_current_t *packet = (mavlink_mission_current_t *)msgbuf; + packet->seq = seq; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_CURRENT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h index a8d60eca7..ef9394f00 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h @@ -289,6 +289,64 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_MISSION_ITEM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, x); + _mav_put_float(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +#else + mavlink_mission_item_t *packet = (mavlink_mission_item_t *)msgbuf; + packet->param1 = param1; + packet->param2 = param2; + packet->param3 = param3; + packet->param4 = param4; + packet->x = x; + packet->y = y; + packet->z = z; + packet->seq = seq; + packet->command = command; + packet->target_system = target_system; + packet->target_component = target_component; + packet->frame = frame; + packet->current = current; + packet->autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_ITEM UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h index f3744fde6..9dfa28043 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h @@ -146,6 +146,38 @@ static inline void mavlink_msg_mission_item_reached_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_item_reached_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +#else + mavlink_mission_item_reached_t *packet = (mavlink_mission_item_reached_t *)msgbuf; + packet->seq = seq; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_ITEM_REACHED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h index ac84e3554..29b0ef6ef 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_MISSION_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +#else + mavlink_mission_request_t *packet = (mavlink_mission_request_t *)msgbuf; + packet->seq = seq; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_REQUEST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h index d999babdb..a275348ac 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +#else + mavlink_mission_request_list_t *packet = (mavlink_mission_request_list_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_REQUEST_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h index 35c7e1285..79a88dc08 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel #endif } +#if MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +#else + mavlink_mission_request_partial_list_t *packet = (mavlink_mission_request_partial_list_t *)msgbuf; + packet->start_index = start_index; + packet->end_index = end_index; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_REQUEST_PARTIAL_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h index 63b37cf8c..0c2a3ada4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_set_current_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_set_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +#else + mavlink_mission_set_current_t *packet = (mavlink_mission_set_current_t *)msgbuf; + packet->seq = seq; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_SET_CURRENT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h index 7de38bd7b..17a5a6bdd 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t #endif } +#if MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +#else + mavlink_mission_write_partial_list_t *packet = (mavlink_mission_write_partial_list_t *)msgbuf; + packet->start_index = start_index; + packet->end_index = end_index; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_WRITE_PARTIAL_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h index fbf4f75c9..df82704a4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_named_value_float_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +#else + mavlink_named_value_float_t *packet = (mavlink_named_value_float_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->value = value; + mav_array_memcpy(packet->name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE NAMED_VALUE_FLOAT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h index 052f24793..cfdd73d61 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_named_value_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, int32_t value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +#else + mavlink_named_value_int_t *packet = (mavlink_named_value_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->value = value; + mav_array_memcpy(packet->name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE NAMED_VALUE_INT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h index e95c144de..b9a748b22 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h @@ -223,6 +223,52 @@ static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_nav_controller_output_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_float(buf, 8, alt_error); + _mav_put_float(buf, 12, aspd_error); + _mav_put_float(buf, 16, xtrack_error); + _mav_put_int16_t(buf, 20, nav_bearing); + _mav_put_int16_t(buf, 22, target_bearing); + _mav_put_uint16_t(buf, 24, wp_dist); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +#else + mavlink_nav_controller_output_t *packet = (mavlink_nav_controller_output_t *)msgbuf; + packet->nav_roll = nav_roll; + packet->nav_pitch = nav_pitch; + packet->alt_error = alt_error; + packet->aspd_error = aspd_error; + packet->xtrack_error = xtrack_error; + packet->nav_bearing = nav_bearing; + packet->target_bearing = target_bearing; + packet->wp_dist = wp_dist; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE NAV_CONTROLLER_OUTPUT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h index 4debb6e66..4ee9c452f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h @@ -196,6 +196,46 @@ static inline void mavlink_msg_omnidirectional_flow_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_omnidirectional_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, const int16_t *left, const int16_t *right, uint8_t quality, float front_distance_m) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, front_distance_m); + _mav_put_uint8_t(buf, 52, sensor_id); + _mav_put_uint8_t(buf, 53, quality); + _mav_put_int16_t_array(buf, 12, left, 10); + _mav_put_int16_t_array(buf, 32, right, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); +#endif +#else + mavlink_omnidirectional_flow_t *packet = (mavlink_omnidirectional_flow_t *)msgbuf; + packet->time_usec = time_usec; + packet->front_distance_m = front_distance_m; + packet->sensor_id = sensor_id; + packet->quality = quality; + mav_array_memcpy(packet->left, left, sizeof(int16_t)*10); + mav_array_memcpy(packet->right, right, sizeof(int16_t)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OMNIDIRECTIONAL_FLOW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h index cf6db9147..6a2efc664 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h @@ -223,6 +223,52 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_ #endif } +#if MAVLINK_MSG_ID_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +#else + mavlink_optical_flow_t *packet = (mavlink_optical_flow_t *)msgbuf; + packet->time_usec = time_usec; + packet->flow_comp_m_x = flow_comp_m_x; + packet->flow_comp_m_y = flow_comp_m_y; + packet->ground_distance = ground_distance; + packet->flow_x = flow_x; + packet->flow_y = flow_y; + packet->sensor_id = sensor_id; + packet->quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OPTICAL_FLOW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h index 39e007274..f9466b002 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, u #endif } +#if MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif +#else + mavlink_param_request_list_t *packet = (mavlink_param_request_list_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PARAM_REQUEST_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h index 5d9113114..730cff066 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u #endif } +#if MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_request_read_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif +#else + mavlink_param_request_read_t *packet = (mavlink_param_request_read_t *)msgbuf; + packet->param_index = param_index; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PARAM_REQUEST_READ UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h index 1bd1f0059..f669af1a2 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h @@ -184,6 +184,44 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta #endif } +#if MAVLINK_MSG_ID_PARAM_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_set_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif +#else + mavlink_param_set_t *packet = (mavlink_param_set_t *)msgbuf; + packet->param_value = param_value; + packet->target_system = target_system; + packet->target_component = target_component; + packet->param_type = param_type; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PARAM_SET UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h index 17c759811..c27957913 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h @@ -184,6 +184,44 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch #endif } +#if MAVLINK_MSG_ID_PARAM_VALUE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_value_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif +#else + mavlink_param_value_t *packet = (mavlink_param_value_t *)msgbuf; + packet->param_value = param_value; + packet->param_count = param_count; + packet->param_index = param_index; + packet->param_type = param_type; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PARAM_VALUE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h index 0c68ca176..b1501ba1f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint64_t time_u #endif } +#if MAVLINK_MSG_ID_PING_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ping_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN); +#endif +#else + mavlink_ping_t *packet = (mavlink_ping_t *)msgbuf; + packet->time_usec = time_usec; + packet->seq = seq; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PING UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h new file mode 100644 index 000000000..1a6259aa3 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h @@ -0,0 +1,257 @@ +// MESSAGE POWER_STATUS PACKING + +#define MAVLINK_MSG_ID_POWER_STATUS 125 + +typedef struct __mavlink_power_status_t +{ + uint16_t Vcc; ///< 5V rail voltage in millivolts + uint16_t Vservo; ///< servo rail voltage in millivolts + uint16_t flags; ///< power supply status flags (see MAV_POWER_STATUS enum) +} mavlink_power_status_t; + +#define MAVLINK_MSG_ID_POWER_STATUS_LEN 6 +#define MAVLINK_MSG_ID_125_LEN 6 + +#define MAVLINK_MSG_ID_POWER_STATUS_CRC 203 +#define MAVLINK_MSG_ID_125_CRC 203 + + + +#define MAVLINK_MESSAGE_INFO_POWER_STATUS { \ + "POWER_STATUS", \ + 3, \ + { { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_power_status_t, Vcc) }, \ + { "Vservo", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_power_status_t, Vservo) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_power_status_t, flags) }, \ + } \ +} + + +/** + * @brief Pack a power_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param Vcc 5V rail voltage in millivolts + * @param Vservo servo rail voltage in millivolts + * @param flags power supply status flags (see MAV_POWER_STATUS enum) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_power_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POWER_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} + +/** + * @brief Pack a power_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param Vcc 5V rail voltage in millivolts + * @param Vservo servo rail voltage in millivolts + * @param flags power supply status flags (see MAV_POWER_STATUS enum) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_power_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t Vcc,uint16_t Vservo,uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POWER_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} + +/** + * @brief Encode a power_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param power_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_power_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_power_status_t* power_status) +{ + return mavlink_msg_power_status_pack(system_id, component_id, msg, power_status->Vcc, power_status->Vservo, power_status->flags); +} + +/** + * @brief Encode a power_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param power_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_power_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_power_status_t* power_status) +{ + return mavlink_msg_power_status_pack_chan(system_id, component_id, chan, msg, power_status->Vcc, power_status->Vservo, power_status->flags); +} + +/** + * @brief Send a power_status message + * @param chan MAVLink channel to send the message + * + * @param Vcc 5V rail voltage in millivolts + * @param Vservo servo rail voltage in millivolts + * @param flags power supply status flags (see MAV_POWER_STATUS enum) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_power_status_send(mavlink_channel_t chan, uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_POWER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_power_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#else + mavlink_power_status_t *packet = (mavlink_power_status_t *)msgbuf; + packet->Vcc = Vcc; + packet->Vservo = Vservo; + packet->flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)packet, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE POWER_STATUS UNPACKING + + +/** + * @brief Get field Vcc from power_status message + * + * @return 5V rail voltage in millivolts + */ +static inline uint16_t mavlink_msg_power_status_get_Vcc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field Vservo from power_status message + * + * @return servo rail voltage in millivolts + */ +static inline uint16_t mavlink_msg_power_status_get_Vservo(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field flags from power_status message + * + * @return power supply status flags (see MAV_POWER_STATUS enum) + */ +static inline uint16_t mavlink_msg_power_status_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Decode a power_status message into a struct + * + * @param msg The message to decode + * @param power_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_power_status_decode(const mavlink_message_t* msg, mavlink_power_status_t* power_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + power_status->Vcc = mavlink_msg_power_status_get_Vcc(msg); + power_status->Vservo = mavlink_msg_power_status_get_Vservo(msg); + power_status->flags = mavlink_msg_power_status_get_flags(msg); +#else + memcpy(power_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h index fceb7d168..5763008fe 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_radio_status_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_RADIO_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_radio_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +#else + mavlink_radio_status_t *packet = (mavlink_radio_status_t *)msgbuf; + packet->rxerrors = rxerrors; + packet->fixed = fixed; + packet->rssi = rssi; + packet->remrssi = remrssi; + packet->txbuf = txbuf; + packet->noise = noise; + packet->remnoise = remnoise; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RADIO_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h index 62a9b6eea..a98e8ce72 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim #endif } +#if MAVLINK_MSG_ID_RAW_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +#else + mavlink_raw_imu_t *packet = (mavlink_raw_imu_t *)msgbuf; + packet->time_usec = time_usec; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RAW_IMU UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h index 82c5fad4a..6bd37fcae 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_ #endif } +#if MAVLINK_MSG_ID_RAW_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_raw_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +#else + mavlink_raw_pressure_t *packet = (mavlink_raw_pressure_t *)msgbuf; + packet->time_usec = time_usec; + packet->press_abs = press_abs; + packet->press_diff1 = press_diff1; + packet->press_diff2 = press_diff2; + packet->temperature = temperature; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RAW_PRESSURE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h new file mode 100644 index 000000000..479af122f --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h @@ -0,0 +1,689 @@ +// MESSAGE RC_CHANNELS PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS 65 + +typedef struct __mavlink_rc_channels_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan9_raw; ///< RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan10_raw; ///< RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan11_raw; ///< RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan12_raw; ///< RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan13_raw; ///< RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan14_raw; ///< RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan15_raw; ///< RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan16_raw; ///< RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan17_raw; ///< RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan18_raw; ///< RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint8_t chancount; ///< Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. +} mavlink_rc_channels_t; + +#define MAVLINK_MSG_ID_RC_CHANNELS_LEN 42 +#define MAVLINK_MSG_ID_65_LEN 42 + +#define MAVLINK_MSG_ID_RC_CHANNELS_CRC 118 +#define MAVLINK_MSG_ID_65_CRC 118 + + + +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS { \ + "RC_CHANNELS", \ + 21, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_t, time_boot_ms) }, \ + { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_t, chan1_raw) }, \ + { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_t, chan2_raw) }, \ + { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_t, chan3_raw) }, \ + { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_t, chan4_raw) }, \ + { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_t, chan5_raw) }, \ + { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_t, chan6_raw) }, \ + { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_rc_channels_t, chan7_raw) }, \ + { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_t, chan8_raw) }, \ + { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_rc_channels_t, chan9_raw) }, \ + { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_rc_channels_t, chan10_raw) }, \ + { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_rc_channels_t, chan11_raw) }, \ + { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_rc_channels_t, chan12_raw) }, \ + { "chan13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_rc_channels_t, chan13_raw) }, \ + { "chan14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_rc_channels_t, chan14_raw) }, \ + { "chan15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_rc_channels_t, chan15_raw) }, \ + { "chan16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_rc_channels_t, chan16_raw) }, \ + { "chan17_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_rc_channels_t, chan17_raw) }, \ + { "chan18_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 38, offsetof(mavlink_rc_channels_t, chan18_raw) }, \ + { "chancount", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_rc_channels_t, chancount) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_rc_channels_t, rssi) }, \ + } \ +} + + +/** + * @brief Pack a rc_channels message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#else + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; + packet.chancount = chancount; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +} + +/** + * @brief Pack a rc_channels message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t chancount,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint16_t chan9_raw,uint16_t chan10_raw,uint16_t chan11_raw,uint16_t chan12_raw,uint16_t chan13_raw,uint16_t chan14_raw,uint16_t chan15_raw,uint16_t chan16_raw,uint16_t chan17_raw,uint16_t chan18_raw,uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#else + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; + packet.chancount = chancount; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +} + +/** + * @brief Encode a rc_channels struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rc_channels C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels) +{ + return mavlink_msg_rc_channels_pack(system_id, component_id, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); +} + +/** + * @brief Encode a rc_channels struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels) +{ + return mavlink_msg_rc_channels_pack_chan(system_id, component_id, chan, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); +} + +/** + * @brief Send a rc_channels message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rc_channels_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#else + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; + packet.chancount = chancount; + packet.rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RC_CHANNELS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#else + mavlink_rc_channels_t *packet = (mavlink_rc_channels_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + packet->chan9_raw = chan9_raw; + packet->chan10_raw = chan10_raw; + packet->chan11_raw = chan11_raw; + packet->chan12_raw = chan12_raw; + packet->chan13_raw = chan13_raw; + packet->chan14_raw = chan14_raw; + packet->chan15_raw = chan15_raw; + packet->chan16_raw = chan16_raw; + packet->chan17_raw = chan17_raw; + packet->chan18_raw = chan18_raw; + packet->chancount = chancount; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RC_CHANNELS UNPACKING + + +/** + * @brief Get field time_boot_ms from rc_channels message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_rc_channels_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field chancount from rc_channels message + * + * @return Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + */ +static inline uint8_t mavlink_msg_rc_channels_get_chancount(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field chan1_raw from rc_channels message + * + * @return RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan1_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field chan2_raw from rc_channels message + * + * @return RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan2_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field chan3_raw from rc_channels message + * + * @return RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan3_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field chan4_raw from rc_channels message + * + * @return RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan4_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field chan5_raw from rc_channels message + * + * @return RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan5_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field chan6_raw from rc_channels message + * + * @return RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan6_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field chan7_raw from rc_channels message + * + * @return RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan7_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field chan8_raw from rc_channels message + * + * @return RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan8_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field chan9_raw from rc_channels message + * + * @return RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan9_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field chan10_raw from rc_channels message + * + * @return RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan10_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 22); +} + +/** + * @brief Get field chan11_raw from rc_channels message + * + * @return RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan11_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field chan12_raw from rc_channels message + * + * @return RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan12_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field chan13_raw from rc_channels message + * + * @return RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan13_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field chan14_raw from rc_channels message + * + * @return RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan14_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 30); +} + +/** + * @brief Get field chan15_raw from rc_channels message + * + * @return RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan15_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 32); +} + +/** + * @brief Get field chan16_raw from rc_channels message + * + * @return RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan16_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 34); +} + +/** + * @brief Get field chan17_raw from rc_channels message + * + * @return RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan17_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 36); +} + +/** + * @brief Get field chan18_raw from rc_channels message + * + * @return RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan18_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 38); +} + +/** + * @brief Get field rssi from rc_channels message + * + * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + */ +static inline uint8_t mavlink_msg_rc_channels_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 41); +} + +/** + * @brief Decode a rc_channels message into a struct + * + * @param msg The message to decode + * @param rc_channels C-struct to decode the message contents into + */ +static inline void mavlink_msg_rc_channels_decode(const mavlink_message_t* msg, mavlink_rc_channels_t* rc_channels) +{ +#if MAVLINK_NEED_BYTE_SWAP + rc_channels->time_boot_ms = mavlink_msg_rc_channels_get_time_boot_ms(msg); + rc_channels->chan1_raw = mavlink_msg_rc_channels_get_chan1_raw(msg); + rc_channels->chan2_raw = mavlink_msg_rc_channels_get_chan2_raw(msg); + rc_channels->chan3_raw = mavlink_msg_rc_channels_get_chan3_raw(msg); + rc_channels->chan4_raw = mavlink_msg_rc_channels_get_chan4_raw(msg); + rc_channels->chan5_raw = mavlink_msg_rc_channels_get_chan5_raw(msg); + rc_channels->chan6_raw = mavlink_msg_rc_channels_get_chan6_raw(msg); + rc_channels->chan7_raw = mavlink_msg_rc_channels_get_chan7_raw(msg); + rc_channels->chan8_raw = mavlink_msg_rc_channels_get_chan8_raw(msg); + rc_channels->chan9_raw = mavlink_msg_rc_channels_get_chan9_raw(msg); + rc_channels->chan10_raw = mavlink_msg_rc_channels_get_chan10_raw(msg); + rc_channels->chan11_raw = mavlink_msg_rc_channels_get_chan11_raw(msg); + rc_channels->chan12_raw = mavlink_msg_rc_channels_get_chan12_raw(msg); + rc_channels->chan13_raw = mavlink_msg_rc_channels_get_chan13_raw(msg); + rc_channels->chan14_raw = mavlink_msg_rc_channels_get_chan14_raw(msg); + rc_channels->chan15_raw = mavlink_msg_rc_channels_get_chan15_raw(msg); + rc_channels->chan16_raw = mavlink_msg_rc_channels_get_chan16_raw(msg); + rc_channels->chan17_raw = mavlink_msg_rc_channels_get_chan17_raw(msg); + rc_channels->chan18_raw = mavlink_msg_rc_channels_get_chan18_raw(msg); + rc_channels->chancount = mavlink_msg_rc_channels_get_chancount(msg); + rc_channels->rssi = mavlink_msg_rc_channels_get_rssi(msg); +#else + memcpy(rc_channels, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h index 0d8a1514b..15d4c6f95 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +#else + mavlink_rc_channels_override_t *packet = (mavlink_rc_channels_override_t *)msgbuf; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RC_CHANNELS_OVERRIDE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h index 3c0aed020..d75a5934a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +#else + mavlink_rc_channels_raw_t *packet = (mavlink_rc_channels_raw_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + packet->port = port; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RC_CHANNELS_RAW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h index 2153ab392..f400f987d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, u #endif } +#if MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_scaled_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, chan1_scaled); + _mav_put_int16_t(buf, 6, chan2_scaled); + _mav_put_int16_t(buf, 8, chan3_scaled); + _mav_put_int16_t(buf, 10, chan4_scaled); + _mav_put_int16_t(buf, 12, chan5_scaled); + _mav_put_int16_t(buf, 14, chan6_scaled); + _mav_put_int16_t(buf, 16, chan7_scaled); + _mav_put_int16_t(buf, 18, chan8_scaled); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +#else + mavlink_rc_channels_scaled_t *packet = (mavlink_rc_channels_scaled_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->chan1_scaled = chan1_scaled; + packet->chan2_scaled = chan2_scaled; + packet->chan3_scaled = chan3_scaled; + packet->chan4_scaled = chan4_scaled; + packet->chan5_scaled = chan5_scaled; + packet->chan6_scaled = chan6_scaled; + packet->chan7_scaled = chan7_scaled; + packet->chan8_scaled = chan8_scaled; + packet->port = port; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RC_CHANNELS_SCALED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h index c754ad876..2ebcc54da 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_request_data_stream_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, req_message_rate); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, req_stream_id); + _mav_put_uint8_t(buf, 5, start_stop); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +#else + mavlink_request_data_stream_t *packet = (mavlink_request_data_stream_t *)msgbuf; + packet->req_message_rate = req_message_rate; + packet->target_system = target_system; + packet->target_component = target_component; + packet->req_stream_id = req_stream_id; + packet->start_stop = start_stop; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE REQUEST_DATA_STREAM UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h index ac3ef4fa9..131b756b6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(mavlink #endif } +#if MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll_rate, float pitch_rate, float yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll_rate); + _mav_put_float(buf, 8, pitch_rate); + _mav_put_float(buf, 12, yaw_rate); + _mav_put_float(buf, 16, thrust); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); +#endif +#else + mavlink_roll_pitch_yaw_rates_thrust_setpoint_t *packet = (mavlink_roll_pitch_yaw_rates_thrust_setpoint_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll_rate = roll_rate; + packet->pitch_rate = pitch_rate; + packet->yaw_rate = yaw_rate; + packet->thrust = thrust; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ROLL_PITCH_YAW_RATES_THRUST_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h index 626477322..757e6d138 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink #endif } +#if MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll_speed); + _mav_put_float(buf, 8, pitch_speed); + _mav_put_float(buf, 12, yaw_speed); + _mav_put_float(buf, 16, thrust); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); +#endif +#else + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *packet = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll_speed = roll_speed; + packet->pitch_speed = pitch_speed; + packet->yaw_speed = yaw_speed; + packet->thrust = thrust; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h index ffcdc547b..f04eedcb3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_chann #endif } +#if MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); +#endif +#else + mavlink_roll_pitch_yaw_thrust_setpoint_t *packet = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->thrust = thrust; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h index fcd54cbb7..0176dfcc8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_safety_allowed_area_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +#else + mavlink_safety_allowed_area_t *packet = (mavlink_safety_allowed_area_t *)msgbuf; + packet->p1x = p1x; + packet->p1y = p1y; + packet->p1z = p1z; + packet->p2x = p2x; + packet->p2y = p2y; + packet->p2z = p2z; + packet->frame = frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SAFETY_ALLOWED_AREA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h index 61f6af155..dafbc260b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_safety_set_allowed_area_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, target_system); + _mav_put_uint8_t(buf, 25, target_component); + _mav_put_uint8_t(buf, 26, frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +#else + mavlink_safety_set_allowed_area_t *packet = (mavlink_safety_set_allowed_area_t *)msgbuf; + packet->p1x = p1x; + packet->p1y = p1y; + packet->p1z = p1z; + packet->p2x = p2x; + packet->p2y = p2y; + packet->p2z = p2z; + packet->target_system = target_system; + packet->target_component = target_component; + packet->frame = frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SAFETY_SET_ALLOWED_AREA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h index 3010d051a..1648a56f8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t #endif } +#if MAVLINK_MSG_ID_SCALED_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +#else + mavlink_scaled_imu_t *packet = (mavlink_scaled_imu_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SCALED_IMU UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h index ea4dbbf81..1dea121dd 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t #endif } +#if MAVLINK_MSG_ID_SCALED_IMU2_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +#else + mavlink_scaled_imu2_t *packet = (mavlink_scaled_imu2_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SCALED_IMU2 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h index 10324bc94..e0d6d8766 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_SCALED_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +#else + mavlink_scaled_pressure_t *packet = (mavlink_scaled_pressure_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->press_abs = press_abs; + packet->press_diff = press_diff; + packet->temperature = temperature; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SCALED_PRESSURE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h new file mode 100644 index 000000000..cbf72bae3 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h @@ -0,0 +1,321 @@ +// MESSAGE SERIAL_CONTROL PACKING + +#define MAVLINK_MSG_ID_SERIAL_CONTROL 126 + +typedef struct __mavlink_serial_control_t +{ + uint32_t baudrate; ///< Baudrate of transfer. Zero means no change. + uint16_t timeout; ///< Timeout for reply data in milliseconds + uint8_t device; ///< See SERIAL_CONTROL_DEV enum + uint8_t flags; ///< See SERIAL_CONTROL_FLAG enum + uint8_t count; ///< how many bytes in this transfer + uint8_t data[70]; ///< serial data +} mavlink_serial_control_t; + +#define MAVLINK_MSG_ID_SERIAL_CONTROL_LEN 79 +#define MAVLINK_MSG_ID_126_LEN 79 + +#define MAVLINK_MSG_ID_SERIAL_CONTROL_CRC 220 +#define MAVLINK_MSG_ID_126_CRC 220 + +#define MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN 70 + +#define MAVLINK_MESSAGE_INFO_SERIAL_CONTROL { \ + "SERIAL_CONTROL", \ + 6, \ + { { "baudrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_control_t, baudrate) }, \ + { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_serial_control_t, timeout) }, \ + { "device", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_serial_control_t, device) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_serial_control_t, flags) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_serial_control_t, count) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 70, 9, offsetof(mavlink_serial_control_t, data) }, \ + } \ +} + + +/** + * @brief Pack a serial_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param device See SERIAL_CONTROL_DEV enum + * @param flags See SERIAL_CONTROL_FLAG enum + * @param timeout Timeout for reply data in milliseconds + * @param baudrate Baudrate of transfer. Zero means no change. + * @param count how many bytes in this transfer + * @param data serial data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_serial_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} + +/** + * @brief Pack a serial_control message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param device See SERIAL_CONTROL_DEV enum + * @param flags See SERIAL_CONTROL_FLAG enum + * @param timeout Timeout for reply data in milliseconds + * @param baudrate Baudrate of transfer. Zero means no change. + * @param count how many bytes in this transfer + * @param data serial data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_serial_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t device,uint8_t flags,uint16_t timeout,uint32_t baudrate,uint8_t count,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} + +/** + * @brief Encode a serial_control struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param serial_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) +{ + return mavlink_msg_serial_control_pack(system_id, component_id, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); +} + +/** + * @brief Encode a serial_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) +{ + return mavlink_msg_serial_control_pack_chan(system_id, component_id, chan, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); +} + +/** + * @brief Send a serial_control message + * @param chan MAVLink channel to send the message + * + * @param device See SERIAL_CONTROL_DEV enum + * @param flags See SERIAL_CONTROL_FLAG enum + * @param timeout Timeout for reply data in milliseconds + * @param baudrate Baudrate of transfer. Zero means no change. + * @param count how many bytes in this transfer + * @param data serial data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_serial_control_send(mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SERIAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#else + mavlink_serial_control_t *packet = (mavlink_serial_control_t *)msgbuf; + packet->baudrate = baudrate; + packet->timeout = timeout; + packet->device = device; + packet->flags = flags; + packet->count = count; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SERIAL_CONTROL UNPACKING + + +/** + * @brief Get field device from serial_control message + * + * @return See SERIAL_CONTROL_DEV enum + */ +static inline uint8_t mavlink_msg_serial_control_get_device(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field flags from serial_control message + * + * @return See SERIAL_CONTROL_FLAG enum + */ +static inline uint8_t mavlink_msg_serial_control_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field timeout from serial_control message + * + * @return Timeout for reply data in milliseconds + */ +static inline uint16_t mavlink_msg_serial_control_get_timeout(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field baudrate from serial_control message + * + * @return Baudrate of transfer. Zero means no change. + */ +static inline uint32_t mavlink_msg_serial_control_get_baudrate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field count from serial_control message + * + * @return how many bytes in this transfer + */ +static inline uint8_t mavlink_msg_serial_control_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field data from serial_control message + * + * @return serial data + */ +static inline uint16_t mavlink_msg_serial_control_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 70, 9); +} + +/** + * @brief Decode a serial_control message into a struct + * + * @param msg The message to decode + * @param serial_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_serial_control_decode(const mavlink_message_t* msg, mavlink_serial_control_t* serial_control) +{ +#if MAVLINK_NEED_BYTE_SWAP + serial_control->baudrate = mavlink_msg_serial_control_get_baudrate(msg); + serial_control->timeout = mavlink_msg_serial_control_get_timeout(msg); + serial_control->device = mavlink_msg_serial_control_get_device(msg); + serial_control->flags = mavlink_msg_serial_control_get_flags(msg); + serial_control->count = mavlink_msg_serial_control_get_count(msg); + mavlink_msg_serial_control_get_data(msg, serial_control->data); +#else + memcpy(serial_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h index 6a14e93ed..e8408862f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 4, servo1_raw); + _mav_put_uint16_t(buf, 6, servo2_raw); + _mav_put_uint16_t(buf, 8, servo3_raw); + _mav_put_uint16_t(buf, 10, servo4_raw); + _mav_put_uint16_t(buf, 12, servo5_raw); + _mav_put_uint16_t(buf, 14, servo6_raw); + _mav_put_uint16_t(buf, 16, servo7_raw); + _mav_put_uint16_t(buf, 18, servo8_raw); + _mav_put_uint8_t(buf, 20, port); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +#else + mavlink_servo_output_raw_t *packet = (mavlink_servo_output_raw_t *)msgbuf; + packet->time_usec = time_usec; + packet->servo1_raw = servo1_raw; + packet->servo2_raw = servo2_raw; + packet->servo3_raw = servo3_raw; + packet->servo4_raw = servo4_raw; + packet->servo5_raw = servo5_raw; + packet->servo6_raw = servo6_raw; + packet->servo7_raw = servo7_raw; + packet->servo8_raw = servo8_raw; + packet->port = port; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERVO_OUTPUT_RAW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h index 6f0d7a69d..94a7b432b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_set_global_position_setpoint_int_send(mavlink_cha #endif } +#if MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_global_position_setpoint_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_int16_t(buf, 12, yaw); + _mav_put_uint8_t(buf, 14, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); +#endif +#else + mavlink_set_global_position_setpoint_int_t *packet = (mavlink_set_global_position_setpoint_int_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude = altitude; + packet->yaw = yaw; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_GLOBAL_POSITION_SETPOINT_INT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h index c444d8d52..1c018cbee 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +#else + mavlink_set_gps_global_origin_t *packet = (mavlink_set_gps_global_origin_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude = altitude; + packet->target_system = target_system; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_GPS_GLOBAL_ORIGIN UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h index 6f2835e03..04dbe6e9a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_set_local_position_setpoint_send(mavlink_channel_ #endif } +#if MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_local_position_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, float x, float y, float z, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint8_t(buf, 18, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); +#endif +#else + mavlink_set_local_position_setpoint_t *packet = (mavlink_set_local_position_setpoint_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->yaw = yaw; + packet->target_system = target_system; + packet->target_component = target_component; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_LOCAL_POSITION_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h index 1aff42cce..4b60a4120 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t tar #endif } +#if MAVLINK_MSG_ID_SET_MODE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_mode_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t base_mode, uint32_t custom_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, base_mode); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +#else + mavlink_set_mode_t *packet = (mavlink_set_mode_t *)msgbuf; + packet->custom_mode = custom_mode; + packet->target_system = target_system; + packet->base_mode = base_mode; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)packet, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)packet, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_MODE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h index 8ceb8888f..8c0e2a014 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_set_quad_motors_setpoint_send(mavlink_channel_t c #endif } +#if MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_quad_motors_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint16_t motor_front_nw, uint16_t motor_right_ne, uint16_t motor_back_se, uint16_t motor_left_sw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, motor_front_nw); + _mav_put_uint16_t(buf, 2, motor_right_ne); + _mav_put_uint16_t(buf, 4, motor_back_se); + _mav_put_uint16_t(buf, 6, motor_left_sw); + _mav_put_uint8_t(buf, 8, target_system); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); +#endif +#else + mavlink_set_quad_motors_setpoint_t *packet = (mavlink_set_quad_motors_setpoint_t *)msgbuf; + packet->motor_front_nw = motor_front_nw; + packet->motor_right_ne = motor_right_ne; + packet->motor_back_se = motor_back_se; + packet->motor_left_sw = motor_left_sw; + packet->target_system = target_system; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_QUAD_MOTORS_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h index 9ef294cc9..c9a084773 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h @@ -234,6 +234,52 @@ static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send(mav #endif } +#if MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t group, uint8_t mode, const uint8_t *led_red, const uint8_t *led_blue, const uint8_t *led_green, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 32, group); + _mav_put_uint8_t(buf, 33, mode); + _mav_put_int16_t_array(buf, 0, roll, 4); + _mav_put_int16_t_array(buf, 8, pitch, 4); + _mav_put_int16_t_array(buf, 16, yaw, 4); + _mav_put_uint16_t_array(buf, 24, thrust, 4); + _mav_put_uint8_t_array(buf, 34, led_red, 4); + _mav_put_uint8_t_array(buf, 38, led_blue, 4); + _mav_put_uint8_t_array(buf, 42, led_green, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); +#endif +#else + mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t *packet = (mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t *)msgbuf; + packet->group = group; + packet->mode = mode; + mav_array_memcpy(packet->roll, roll, sizeof(int16_t)*4); + mav_array_memcpy(packet->pitch, pitch, sizeof(int16_t)*4); + mav_array_memcpy(packet->yaw, yaw, sizeof(int16_t)*4); + mav_array_memcpy(packet->thrust, thrust, sizeof(uint16_t)*4); + mav_array_memcpy(packet->led_red, led_red, sizeof(uint8_t)*4); + mav_array_memcpy(packet->led_blue, led_blue, sizeof(uint8_t)*4); + mav_array_memcpy(packet->led_green, led_green, sizeof(uint8_t)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h index 7d8d526f8..338bf0e43 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h @@ -198,6 +198,46 @@ static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_send(mavlink #endif } +#if MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t group, uint8_t mode, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 32, group); + _mav_put_uint8_t(buf, 33, mode); + _mav_put_int16_t_array(buf, 0, roll, 4); + _mav_put_int16_t_array(buf, 8, pitch, 4); + _mav_put_int16_t_array(buf, 16, yaw, 4); + _mav_put_uint16_t_array(buf, 24, thrust, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); +#endif +#else + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t *packet = (mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t *)msgbuf; + packet->group = group; + packet->mode = mode; + mav_array_memcpy(packet->roll, roll, sizeof(int16_t)*4); + mav_array_memcpy(packet->pitch, pitch, sizeof(int16_t)*4); + mav_array_memcpy(packet->yaw, yaw, sizeof(int16_t)*4); + mav_array_memcpy(packet->thrust, thrust, sizeof(uint16_t)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h index 5846ba41f..4d0e5931f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_chan #endif } +#if MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll_speed); + _mav_put_float(buf, 4, pitch_speed); + _mav_put_float(buf, 8, yaw_speed); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); +#endif +#else + mavlink_set_roll_pitch_yaw_speed_thrust_t *packet = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)msgbuf; + packet->roll_speed = roll_speed; + packet->pitch_speed = pitch_speed; + packet->yaw_speed = yaw_speed; + packet->thrust = thrust; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h index 334fd39e3..21e0a43f2 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t #endif } +#if MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); +#endif +#else + mavlink_set_roll_pitch_yaw_thrust_t *packet = (mavlink_set_roll_pitch_yaw_thrust_t *)msgbuf; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->thrust = thrust; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_ROLL_PITCH_YAW_THRUST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h index 93ce345b6..90b63427b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_setpoint_6dof_send(mavlink_channel_t chan, uint8_ #endif } +#if MAVLINK_MSG_ID_SETPOINT_6DOF_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_setpoint_6dof_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, trans_x); + _mav_put_float(buf, 4, trans_y); + _mav_put_float(buf, 8, trans_z); + _mav_put_float(buf, 12, rot_x); + _mav_put_float(buf, 16, rot_y); + _mav_put_float(buf, 20, rot_z); + _mav_put_uint8_t(buf, 24, target_system); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); +#endif +#else + mavlink_setpoint_6dof_t *packet = (mavlink_setpoint_6dof_t *)msgbuf; + packet->trans_x = trans_x; + packet->trans_y = trans_y; + packet->trans_z = trans_z; + packet->rot_x = rot_x; + packet->rot_y = rot_y; + packet->rot_z = rot_z; + packet->target_system = target_system; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SETPOINT_6DOF UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h index de80e4645..308a211cd 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_setpoint_8dof_send(mavlink_channel_t chan, uint8_ #endif } +#if MAVLINK_MSG_ID_SETPOINT_8DOF_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_setpoint_8dof_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, val1); + _mav_put_float(buf, 4, val2); + _mav_put_float(buf, 8, val3); + _mav_put_float(buf, 12, val4); + _mav_put_float(buf, 16, val5); + _mav_put_float(buf, 20, val6); + _mav_put_float(buf, 24, val7); + _mav_put_float(buf, 28, val8); + _mav_put_uint8_t(buf, 32, target_system); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); +#endif +#else + mavlink_setpoint_8dof_t *packet = (mavlink_setpoint_8dof_t *)msgbuf; + packet->val1 = val1; + packet->val2 = val2; + packet->val3 = val3; + packet->val4 = val4; + packet->val5 = val5; + packet->val6 = val6; + packet->val7 = val7; + packet->val8 = val8; + packet->target_system = target_system; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SETPOINT_8DOF UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h index ebd657cf3..db3a92642 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h @@ -366,6 +366,78 @@ static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1, #endif } +#if MAVLINK_MSG_ID_SIM_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sim_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, q1); + _mav_put_float(buf, 4, q2); + _mav_put_float(buf, 8, q3); + _mav_put_float(buf, 12, q4); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + _mav_put_float(buf, 28, xacc); + _mav_put_float(buf, 32, yacc); + _mav_put_float(buf, 36, zacc); + _mav_put_float(buf, 40, xgyro); + _mav_put_float(buf, 44, ygyro); + _mav_put_float(buf, 48, zgyro); + _mav_put_float(buf, 52, lat); + _mav_put_float(buf, 56, lon); + _mav_put_float(buf, 60, alt); + _mav_put_float(buf, 64, std_dev_horz); + _mav_put_float(buf, 68, std_dev_vert); + _mav_put_float(buf, 72, vn); + _mav_put_float(buf, 76, ve); + _mav_put_float(buf, 80, vd); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +#else + mavlink_sim_state_t *packet = (mavlink_sim_state_t *)msgbuf; + packet->q1 = q1; + packet->q2 = q2; + packet->q3 = q3; + packet->q4 = q4; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->std_dev_horz = std_dev_horz; + packet->std_dev_vert = std_dev_vert; + packet->vn = vn; + packet->ve = ve; + packet->vd = vd; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SIM_STATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h index 2db627b96..be2629dd8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, flo #endif } +#if MAVLINK_MSG_ID_STATE_CORRECTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_state_correction_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, xErr); + _mav_put_float(buf, 4, yErr); + _mav_put_float(buf, 8, zErr); + _mav_put_float(buf, 12, rollErr); + _mav_put_float(buf, 16, pitchErr); + _mav_put_float(buf, 20, yawErr); + _mav_put_float(buf, 24, vxErr); + _mav_put_float(buf, 28, vyErr); + _mav_put_float(buf, 32, vzErr); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); +#endif +#else + mavlink_state_correction_t *packet = (mavlink_state_correction_t *)msgbuf; + packet->xErr = xErr; + packet->yErr = yErr; + packet->zErr = zErr; + packet->rollErr = rollErr; + packet->pitchErr = pitchErr; + packet->yawErr = yawErr; + packet->vxErr = vxErr; + packet->vyErr = vyErr; + packet->vzErr = vzErr; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE STATE_CORRECTION UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h index 536ca0634..c8c2547b3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h @@ -151,6 +151,38 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s #endif } +#if MAVLINK_MSG_ID_STATUSTEXT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t severity, const char *text) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_char_array(buf, 1, text, 50); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif +#else + mavlink_statustext_t *packet = (mavlink_statustext_t *)msgbuf; + packet->severity = severity; + mav_array_memcpy(packet->text, text, sizeof(char)*50); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE STATUSTEXT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h index 101b36678..1b1730d5f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h @@ -278,6 +278,62 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t #endif } +#if MAVLINK_MSG_ID_SYS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sys_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); + _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); + _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); + _mav_put_uint16_t(buf, 12, load); + _mav_put_uint16_t(buf, 14, voltage_battery); + _mav_put_int16_t(buf, 16, current_battery); + _mav_put_uint16_t(buf, 18, drop_rate_comm); + _mav_put_uint16_t(buf, 20, errors_comm); + _mav_put_uint16_t(buf, 22, errors_count1); + _mav_put_uint16_t(buf, 24, errors_count2); + _mav_put_uint16_t(buf, 26, errors_count3); + _mav_put_uint16_t(buf, 28, errors_count4); + _mav_put_int8_t(buf, 30, battery_remaining); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +#else + mavlink_sys_status_t *packet = (mavlink_sys_status_t *)msgbuf; + packet->onboard_control_sensors_present = onboard_control_sensors_present; + packet->onboard_control_sensors_enabled = onboard_control_sensors_enabled; + packet->onboard_control_sensors_health = onboard_control_sensors_health; + packet->load = load; + packet->voltage_battery = voltage_battery; + packet->current_battery = current_battery; + packet->drop_rate_comm = drop_rate_comm; + packet->errors_comm = errors_comm; + packet->errors_count1 = errors_count1; + packet->errors_count2 = errors_count2; + packet->errors_count3 = errors_count3; + packet->errors_count4 = errors_count4; + packet->battery_remaining = battery_remaining; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SYS_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h index 1807567ae..988c669c3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t #endif } +#if MAVLINK_MSG_ID_SYSTEM_TIME_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_system_time_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_unix_usec, uint32_t time_boot_ms) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_unix_usec); + _mav_put_uint32_t(buf, 8, time_boot_ms); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +#else + mavlink_system_time_t *packet = (mavlink_system_time_t *)msgbuf; + packet->time_unix_usec = time_unix_usec; + packet->time_boot_ms = time_boot_ms; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SYSTEM_TIME UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h index 5b1093a3d..b130ee50b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspe #endif } +#if MAVLINK_MSG_ID_VFR_HUD_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vfr_hud_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, climb); + _mav_put_int16_t(buf, 16, heading); + _mav_put_uint16_t(buf, 18, throttle); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +#else + mavlink_vfr_hud_t *packet = (mavlink_vfr_hud_t *)msgbuf; + packet->airspeed = airspeed; + packet->groundspeed = groundspeed; + packet->alt = alt; + packet->climb = climb; + packet->heading = heading; + packet->throttle = throttle; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE VFR_HUD UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h index a254202e4..b3fa7bc1c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_vicon_position_estimate_t *packet = (mavlink_vicon_position_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE VICON_POSITION_ESTIMATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h index f7a741b09..8f82fb682 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c #endif } +#if MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_vision_position_estimate_t *packet = (mavlink_vision_position_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE VISION_POSITION_ESTIMATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h index 660225128..7528014c7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +#else + mavlink_vision_speed_estimate_t *packet = (mavlink_vision_speed_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE VISION_SPEED_ESTIMATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h index c5aa9ddf3..de23fcb2a 100644 --- a/mavlink/include/mavlink/v1.0/common/testsuite.h +++ b/mavlink/include/mavlink/v1.0/common/testsuite.h @@ -2805,6 +2805,89 @@ static void mavlink_test_state_correction(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_rc_channels(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_rc_channels_t packet_in = { + 963497464, + }17443, + }17547, + }17651, + }17755, + }17859, + }17963, + }18067, + }18171, + }18275, + }18379, + }18483, + }18587, + }18691, + }18795, + }18899, + }19003, + }19107, + }19211, + }125, + }192, + }; + mavlink_rc_channels_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.chan1_raw = packet_in.chan1_raw; + packet1.chan2_raw = packet_in.chan2_raw; + packet1.chan3_raw = packet_in.chan3_raw; + packet1.chan4_raw = packet_in.chan4_raw; + packet1.chan5_raw = packet_in.chan5_raw; + packet1.chan6_raw = packet_in.chan6_raw; + packet1.chan7_raw = packet_in.chan7_raw; + packet1.chan8_raw = packet_in.chan8_raw; + packet1.chan9_raw = packet_in.chan9_raw; + packet1.chan10_raw = packet_in.chan10_raw; + packet1.chan11_raw = packet_in.chan11_raw; + packet1.chan12_raw = packet_in.chan12_raw; + packet1.chan13_raw = packet_in.chan13_raw; + packet1.chan14_raw = packet_in.chan14_raw; + packet1.chan15_raw = packet_in.chan15_raw; + packet1.chan16_raw = packet_in.chan16_raw; + packet1.chan17_raw = packet_in.chan17_raw; + packet1.chan18_raw = packet_in.chan18_raw; + packet1.chancount = packet_in.chancount; + packet1.rssi = packet_in.rssi; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rc_channels_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.chancount , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw , packet1.rssi ); + mavlink_msg_rc_channels_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.chancount , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw , packet1.rssi ); + mavlink_msg_rc_channels_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h index bc47a547a..c834a3df0 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_airspeeds_send(mavlink_channel_t chan, uint32_t t #endif } +#if MAVLINK_MSG_ID_AIRSPEEDS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_airspeeds_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t airspeed_imu, int16_t airspeed_pitot, int16_t airspeed_hot_wire, int16_t airspeed_ultrasonic, int16_t aoa, int16_t aoy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, airspeed_imu); + _mav_put_int16_t(buf, 6, airspeed_pitot); + _mav_put_int16_t(buf, 8, airspeed_hot_wire); + _mav_put_int16_t(buf, 10, airspeed_ultrasonic); + _mav_put_int16_t(buf, 12, aoa); + _mav_put_int16_t(buf, 14, aoy); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN); +#endif +#else + mavlink_airspeeds_t *packet = (mavlink_airspeeds_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->airspeed_imu = airspeed_imu; + packet->airspeed_pitot = airspeed_pitot; + packet->airspeed_hot_wire = airspeed_hot_wire; + packet->airspeed_ultrasonic = airspeed_ultrasonic; + packet->aoa = aoa; + packet->aoy = aoy; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AIRSPEEDS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h index e64787cc7..b009068ff 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_altitudes_send(mavlink_channel_t chan, uint32_t t #endif } +#if MAVLINK_MSG_ID_ALTITUDES_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_altitudes_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t alt_gps, int32_t alt_imu, int32_t alt_barometric, int32_t alt_optical_flow, int32_t alt_range_finder, int32_t alt_extra) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, alt_gps); + _mav_put_int32_t(buf, 8, alt_imu); + _mav_put_int32_t(buf, 12, alt_barometric); + _mav_put_int32_t(buf, 16, alt_optical_flow); + _mav_put_int32_t(buf, 20, alt_range_finder); + _mav_put_int32_t(buf, 24, alt_extra); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, MAVLINK_MSG_ID_ALTITUDES_LEN); +#endif +#else + mavlink_altitudes_t *packet = (mavlink_altitudes_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->alt_gps = alt_gps; + packet->alt_imu = alt_imu; + packet->alt_barometric = alt_barometric; + packet->alt_optical_flow = alt_optical_flow; + packet->alt_range_finder = alt_range_finder; + packet->alt_extra = alt_extra; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)packet, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)packet, MAVLINK_MSG_ID_ALTITUDES_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ALTITUDES UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h index 581bd35dc..e7b803a14 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h @@ -206,6 +206,48 @@ static inline void mavlink_msg_flexifunction_buffer_function_send(mavlink_channe #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_buffer_function_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t func_count, uint16_t data_address, uint16_t data_size, const int8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, func_index); + _mav_put_uint16_t(buf, 2, func_count); + _mav_put_uint16_t(buf, 4, data_address); + _mav_put_uint16_t(buf, 6, data_size); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_int8_t_array(buf, 10, data, 48); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); +#endif +#else + mavlink_flexifunction_buffer_function_t *packet = (mavlink_flexifunction_buffer_function_t *)msgbuf; + packet->func_index = func_index; + packet->func_count = func_count; + packet->data_address = data_address; + packet->data_size = data_size; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->data, data, sizeof(int8_t)*48); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h index 790afd52b..06631b40e 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_flexifunction_buffer_function_ack_send(mavlink_ch #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_buffer_function_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, func_index); + _mav_put_uint16_t(buf, 2, result); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); +#endif +#else + mavlink_flexifunction_buffer_function_ack_t *packet = (mavlink_flexifunction_buffer_function_ack_t *)msgbuf; + packet->func_index = func_index; + packet->result = result; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h index ce722c8a4..ac8366eef 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_flexifunction_command_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_command_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, command_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); +#endif +#else + mavlink_flexifunction_command_t *packet = (mavlink_flexifunction_command_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->command_type = command_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_COMMAND UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h index 070dc4bf8..c88e2dc39 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_flexifunction_command_ack_send(mavlink_channel_t #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t command_type, uint16_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, command_type); + _mav_put_uint16_t(buf, 2, result); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); +#endif +#else + mavlink_flexifunction_command_ack_t *packet = (mavlink_flexifunction_command_ack_t *)msgbuf; + packet->command_type = command_type; + packet->result = result; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_COMMAND_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h index ef262a6b1..619f810c5 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h @@ -195,6 +195,46 @@ static inline void mavlink_msg_flexifunction_directory_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_directory_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, const int8_t *directory_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, directory_type); + _mav_put_uint8_t(buf, 3, start_index); + _mav_put_uint8_t(buf, 4, count); + _mav_put_int8_t_array(buf, 5, directory_data, 48); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); +#endif +#else + mavlink_flexifunction_directory_t *packet = (mavlink_flexifunction_directory_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->directory_type = directory_type; + packet->start_index = start_index; + packet->count = count; + mav_array_memcpy(packet->directory_data, directory_data, sizeof(int8_t)*48); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_DIRECTORY UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h index d3a386ce5..0287183d8 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_flexifunction_directory_ack_send(mavlink_channel_ #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_directory_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, uint16_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, result); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, directory_type); + _mav_put_uint8_t(buf, 5, start_index); + _mav_put_uint8_t(buf, 6, count); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); +#endif +#else + mavlink_flexifunction_directory_ack_t *packet = (mavlink_flexifunction_directory_ack_t *)msgbuf; + packet->result = result; + packet->target_system = target_system; + packet->target_component = target_component; + packet->directory_type = directory_type; + packet->start_index = start_index; + packet->count = count; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_DIRECTORY_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h index e50f77b08..570782e79 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_flexifunction_read_req_send(mavlink_channel_t cha #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_read_req_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t read_req_type, int16_t data_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, read_req_type); + _mav_put_int16_t(buf, 2, data_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); +#endif +#else + mavlink_flexifunction_read_req_t *packet = (mavlink_flexifunction_read_req_t *)msgbuf; + packet->read_req_type = read_req_type; + packet->data_index = data_index; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_READ_REQ UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h index 41afb3811..c10000640 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_flexifunction_set_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_set_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); +#endif +#else + mavlink_flexifunction_set_t *packet = (mavlink_flexifunction_set_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_SET UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h index d21ab4816..bbb753a06 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_serial_udb_extra_f13_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f13_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int16_t sue_week_no, int32_t sue_lat_origin, int32_t sue_lon_origin, int32_t sue_alt_origin) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, sue_lat_origin); + _mav_put_int32_t(buf, 4, sue_lon_origin); + _mav_put_int32_t(buf, 8, sue_alt_origin); + _mav_put_int16_t(buf, 12, sue_week_no); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); +#endif +#else + mavlink_serial_udb_extra_f13_t *packet = (mavlink_serial_udb_extra_f13_t *)msgbuf; + packet->sue_lat_origin = sue_lat_origin; + packet->sue_lon_origin = sue_lon_origin; + packet->sue_alt_origin = sue_alt_origin; + packet->sue_week_no = sue_week_no; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F13 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h index 6ac12f28a..112564d00 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_serial_udb_extra_f14_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f14_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t sue_WIND_ESTIMATION, uint8_t sue_GPS_TYPE, uint8_t sue_DR, uint8_t sue_BOARD_TYPE, uint8_t sue_AIRFRAME, int16_t sue_RCON, int16_t sue_TRAP_FLAGS, uint32_t sue_TRAP_SOURCE, int16_t sue_osc_fail_count, uint8_t sue_CLOCK_CONFIG, uint8_t sue_FLIGHT_PLAN_TYPE) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, sue_TRAP_SOURCE); + _mav_put_int16_t(buf, 4, sue_RCON); + _mav_put_int16_t(buf, 6, sue_TRAP_FLAGS); + _mav_put_int16_t(buf, 8, sue_osc_fail_count); + _mav_put_uint8_t(buf, 10, sue_WIND_ESTIMATION); + _mav_put_uint8_t(buf, 11, sue_GPS_TYPE); + _mav_put_uint8_t(buf, 12, sue_DR); + _mav_put_uint8_t(buf, 13, sue_BOARD_TYPE); + _mav_put_uint8_t(buf, 14, sue_AIRFRAME); + _mav_put_uint8_t(buf, 15, sue_CLOCK_CONFIG); + _mav_put_uint8_t(buf, 16, sue_FLIGHT_PLAN_TYPE); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); +#endif +#else + mavlink_serial_udb_extra_f14_t *packet = (mavlink_serial_udb_extra_f14_t *)msgbuf; + packet->sue_TRAP_SOURCE = sue_TRAP_SOURCE; + packet->sue_RCON = sue_RCON; + packet->sue_TRAP_FLAGS = sue_TRAP_FLAGS; + packet->sue_osc_fail_count = sue_osc_fail_count; + packet->sue_WIND_ESTIMATION = sue_WIND_ESTIMATION; + packet->sue_GPS_TYPE = sue_GPS_TYPE; + packet->sue_DR = sue_DR; + packet->sue_BOARD_TYPE = sue_BOARD_TYPE; + packet->sue_AIRFRAME = sue_AIRFRAME; + packet->sue_CLOCK_CONFIG = sue_CLOCK_CONFIG; + packet->sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F14 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h index 10c3f4ca4..77666407d 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h @@ -158,6 +158,40 @@ static inline void mavlink_msg_serial_udb_extra_f15_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f15_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const uint8_t *sue_ID_VEHICLE_MODEL_NAME, const uint8_t *sue_ID_VEHICLE_REGISTRATION) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_uint8_t_array(buf, 0, sue_ID_VEHICLE_MODEL_NAME, 40); + _mav_put_uint8_t_array(buf, 40, sue_ID_VEHICLE_REGISTRATION, 20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); +#endif +#else + mavlink_serial_udb_extra_f15_t *packet = (mavlink_serial_udb_extra_f15_t *)msgbuf; + + mav_array_memcpy(packet->sue_ID_VEHICLE_MODEL_NAME, sue_ID_VEHICLE_MODEL_NAME, sizeof(uint8_t)*40); + mav_array_memcpy(packet->sue_ID_VEHICLE_REGISTRATION, sue_ID_VEHICLE_REGISTRATION, sizeof(uint8_t)*20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F15 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h index 659e6b7c5..ca9f0556f 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h @@ -158,6 +158,40 @@ static inline void mavlink_msg_serial_udb_extra_f16_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f16_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const uint8_t *sue_ID_LEAD_PILOT, const uint8_t *sue_ID_DIY_DRONES_URL) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_uint8_t_array(buf, 0, sue_ID_LEAD_PILOT, 40); + _mav_put_uint8_t_array(buf, 40, sue_ID_DIY_DRONES_URL, 70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); +#endif +#else + mavlink_serial_udb_extra_f16_t *packet = (mavlink_serial_udb_extra_f16_t *)msgbuf; + + mav_array_memcpy(packet->sue_ID_LEAD_PILOT, sue_ID_LEAD_PILOT, sizeof(uint8_t)*40); + mav_array_memcpy(packet->sue_ID_DIY_DRONES_URL, sue_ID_DIY_DRONES_URL, sizeof(uint8_t)*70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F16 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h index 15ba68a34..41b9f09c0 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h @@ -443,6 +443,92 @@ static inline void mavlink_msg_serial_udb_extra_f2_a_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f2_a_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t sue_time, uint8_t sue_status, int32_t sue_latitude, int32_t sue_longitude, int32_t sue_altitude, uint16_t sue_waypoint_index, int16_t sue_rmat0, int16_t sue_rmat1, int16_t sue_rmat2, int16_t sue_rmat3, int16_t sue_rmat4, int16_t sue_rmat5, int16_t sue_rmat6, int16_t sue_rmat7, int16_t sue_rmat8, uint16_t sue_cog, int16_t sue_sog, uint16_t sue_cpu_load, int16_t sue_voltage_milis, uint16_t sue_air_speed_3DIMU, int16_t sue_estimated_wind_0, int16_t sue_estimated_wind_1, int16_t sue_estimated_wind_2, int16_t sue_magFieldEarth0, int16_t sue_magFieldEarth1, int16_t sue_magFieldEarth2, int16_t sue_svs, int16_t sue_hdop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, sue_time); + _mav_put_int32_t(buf, 4, sue_latitude); + _mav_put_int32_t(buf, 8, sue_longitude); + _mav_put_int32_t(buf, 12, sue_altitude); + _mav_put_uint16_t(buf, 16, sue_waypoint_index); + _mav_put_int16_t(buf, 18, sue_rmat0); + _mav_put_int16_t(buf, 20, sue_rmat1); + _mav_put_int16_t(buf, 22, sue_rmat2); + _mav_put_int16_t(buf, 24, sue_rmat3); + _mav_put_int16_t(buf, 26, sue_rmat4); + _mav_put_int16_t(buf, 28, sue_rmat5); + _mav_put_int16_t(buf, 30, sue_rmat6); + _mav_put_int16_t(buf, 32, sue_rmat7); + _mav_put_int16_t(buf, 34, sue_rmat8); + _mav_put_uint16_t(buf, 36, sue_cog); + _mav_put_int16_t(buf, 38, sue_sog); + _mav_put_uint16_t(buf, 40, sue_cpu_load); + _mav_put_int16_t(buf, 42, sue_voltage_milis); + _mav_put_uint16_t(buf, 44, sue_air_speed_3DIMU); + _mav_put_int16_t(buf, 46, sue_estimated_wind_0); + _mav_put_int16_t(buf, 48, sue_estimated_wind_1); + _mav_put_int16_t(buf, 50, sue_estimated_wind_2); + _mav_put_int16_t(buf, 52, sue_magFieldEarth0); + _mav_put_int16_t(buf, 54, sue_magFieldEarth1); + _mav_put_int16_t(buf, 56, sue_magFieldEarth2); + _mav_put_int16_t(buf, 58, sue_svs); + _mav_put_int16_t(buf, 60, sue_hdop); + _mav_put_uint8_t(buf, 62, sue_status); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); +#endif +#else + mavlink_serial_udb_extra_f2_a_t *packet = (mavlink_serial_udb_extra_f2_a_t *)msgbuf; + packet->sue_time = sue_time; + packet->sue_latitude = sue_latitude; + packet->sue_longitude = sue_longitude; + packet->sue_altitude = sue_altitude; + packet->sue_waypoint_index = sue_waypoint_index; + packet->sue_rmat0 = sue_rmat0; + packet->sue_rmat1 = sue_rmat1; + packet->sue_rmat2 = sue_rmat2; + packet->sue_rmat3 = sue_rmat3; + packet->sue_rmat4 = sue_rmat4; + packet->sue_rmat5 = sue_rmat5; + packet->sue_rmat6 = sue_rmat6; + packet->sue_rmat7 = sue_rmat7; + packet->sue_rmat8 = sue_rmat8; + packet->sue_cog = sue_cog; + packet->sue_sog = sue_sog; + packet->sue_cpu_load = sue_cpu_load; + packet->sue_voltage_milis = sue_voltage_milis; + packet->sue_air_speed_3DIMU = sue_air_speed_3DIMU; + packet->sue_estimated_wind_0 = sue_estimated_wind_0; + packet->sue_estimated_wind_1 = sue_estimated_wind_1; + packet->sue_estimated_wind_2 = sue_estimated_wind_2; + packet->sue_magFieldEarth0 = sue_magFieldEarth0; + packet->sue_magFieldEarth1 = sue_magFieldEarth1; + packet->sue_magFieldEarth2 = sue_magFieldEarth2; + packet->sue_svs = sue_svs; + packet->sue_hdop = sue_hdop; + packet->sue_status = sue_status; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F2_A UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h index 7cb8c87da..3e1757f74 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h @@ -498,6 +498,102 @@ static inline void mavlink_msg_serial_udb_extra_f2_b_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f2_b_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t sue_time, int16_t sue_pwm_input_1, int16_t sue_pwm_input_2, int16_t sue_pwm_input_3, int16_t sue_pwm_input_4, int16_t sue_pwm_input_5, int16_t sue_pwm_input_6, int16_t sue_pwm_input_7, int16_t sue_pwm_input_8, int16_t sue_pwm_input_9, int16_t sue_pwm_input_10, int16_t sue_pwm_output_1, int16_t sue_pwm_output_2, int16_t sue_pwm_output_3, int16_t sue_pwm_output_4, int16_t sue_pwm_output_5, int16_t sue_pwm_output_6, int16_t sue_pwm_output_7, int16_t sue_pwm_output_8, int16_t sue_pwm_output_9, int16_t sue_pwm_output_10, int16_t sue_imu_location_x, int16_t sue_imu_location_y, int16_t sue_imu_location_z, uint32_t sue_flags, int16_t sue_osc_fails, int16_t sue_imu_velocity_x, int16_t sue_imu_velocity_y, int16_t sue_imu_velocity_z, int16_t sue_waypoint_goal_x, int16_t sue_waypoint_goal_y, int16_t sue_waypoint_goal_z, int16_t sue_memory_stack_free) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, sue_time); + _mav_put_uint32_t(buf, 4, sue_flags); + _mav_put_int16_t(buf, 8, sue_pwm_input_1); + _mav_put_int16_t(buf, 10, sue_pwm_input_2); + _mav_put_int16_t(buf, 12, sue_pwm_input_3); + _mav_put_int16_t(buf, 14, sue_pwm_input_4); + _mav_put_int16_t(buf, 16, sue_pwm_input_5); + _mav_put_int16_t(buf, 18, sue_pwm_input_6); + _mav_put_int16_t(buf, 20, sue_pwm_input_7); + _mav_put_int16_t(buf, 22, sue_pwm_input_8); + _mav_put_int16_t(buf, 24, sue_pwm_input_9); + _mav_put_int16_t(buf, 26, sue_pwm_input_10); + _mav_put_int16_t(buf, 28, sue_pwm_output_1); + _mav_put_int16_t(buf, 30, sue_pwm_output_2); + _mav_put_int16_t(buf, 32, sue_pwm_output_3); + _mav_put_int16_t(buf, 34, sue_pwm_output_4); + _mav_put_int16_t(buf, 36, sue_pwm_output_5); + _mav_put_int16_t(buf, 38, sue_pwm_output_6); + _mav_put_int16_t(buf, 40, sue_pwm_output_7); + _mav_put_int16_t(buf, 42, sue_pwm_output_8); + _mav_put_int16_t(buf, 44, sue_pwm_output_9); + _mav_put_int16_t(buf, 46, sue_pwm_output_10); + _mav_put_int16_t(buf, 48, sue_imu_location_x); + _mav_put_int16_t(buf, 50, sue_imu_location_y); + _mav_put_int16_t(buf, 52, sue_imu_location_z); + _mav_put_int16_t(buf, 54, sue_osc_fails); + _mav_put_int16_t(buf, 56, sue_imu_velocity_x); + _mav_put_int16_t(buf, 58, sue_imu_velocity_y); + _mav_put_int16_t(buf, 60, sue_imu_velocity_z); + _mav_put_int16_t(buf, 62, sue_waypoint_goal_x); + _mav_put_int16_t(buf, 64, sue_waypoint_goal_y); + _mav_put_int16_t(buf, 66, sue_waypoint_goal_z); + _mav_put_int16_t(buf, 68, sue_memory_stack_free); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); +#endif +#else + mavlink_serial_udb_extra_f2_b_t *packet = (mavlink_serial_udb_extra_f2_b_t *)msgbuf; + packet->sue_time = sue_time; + packet->sue_flags = sue_flags; + packet->sue_pwm_input_1 = sue_pwm_input_1; + packet->sue_pwm_input_2 = sue_pwm_input_2; + packet->sue_pwm_input_3 = sue_pwm_input_3; + packet->sue_pwm_input_4 = sue_pwm_input_4; + packet->sue_pwm_input_5 = sue_pwm_input_5; + packet->sue_pwm_input_6 = sue_pwm_input_6; + packet->sue_pwm_input_7 = sue_pwm_input_7; + packet->sue_pwm_input_8 = sue_pwm_input_8; + packet->sue_pwm_input_9 = sue_pwm_input_9; + packet->sue_pwm_input_10 = sue_pwm_input_10; + packet->sue_pwm_output_1 = sue_pwm_output_1; + packet->sue_pwm_output_2 = sue_pwm_output_2; + packet->sue_pwm_output_3 = sue_pwm_output_3; + packet->sue_pwm_output_4 = sue_pwm_output_4; + packet->sue_pwm_output_5 = sue_pwm_output_5; + packet->sue_pwm_output_6 = sue_pwm_output_6; + packet->sue_pwm_output_7 = sue_pwm_output_7; + packet->sue_pwm_output_8 = sue_pwm_output_8; + packet->sue_pwm_output_9 = sue_pwm_output_9; + packet->sue_pwm_output_10 = sue_pwm_output_10; + packet->sue_imu_location_x = sue_imu_location_x; + packet->sue_imu_location_y = sue_imu_location_y; + packet->sue_imu_location_z = sue_imu_location_z; + packet->sue_osc_fails = sue_osc_fails; + packet->sue_imu_velocity_x = sue_imu_velocity_x; + packet->sue_imu_velocity_y = sue_imu_velocity_y; + packet->sue_imu_velocity_z = sue_imu_velocity_z; + packet->sue_waypoint_goal_x = sue_waypoint_goal_x; + packet->sue_waypoint_goal_y = sue_waypoint_goal_y; + packet->sue_waypoint_goal_z = sue_waypoint_goal_z; + packet->sue_memory_stack_free = sue_memory_stack_free; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F2_B UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h index 77c616cc2..edd1e924c 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_serial_udb_extra_f4_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f4_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t sue_ROLL_STABILIZATION_AILERONS, uint8_t sue_ROLL_STABILIZATION_RUDDER, uint8_t sue_PITCH_STABILIZATION, uint8_t sue_YAW_STABILIZATION_RUDDER, uint8_t sue_YAW_STABILIZATION_AILERON, uint8_t sue_AILERON_NAVIGATION, uint8_t sue_RUDDER_NAVIGATION, uint8_t sue_ALTITUDEHOLD_STABILIZED, uint8_t sue_ALTITUDEHOLD_WAYPOINT, uint8_t sue_RACING_MODE) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, sue_ROLL_STABILIZATION_AILERONS); + _mav_put_uint8_t(buf, 1, sue_ROLL_STABILIZATION_RUDDER); + _mav_put_uint8_t(buf, 2, sue_PITCH_STABILIZATION); + _mav_put_uint8_t(buf, 3, sue_YAW_STABILIZATION_RUDDER); + _mav_put_uint8_t(buf, 4, sue_YAW_STABILIZATION_AILERON); + _mav_put_uint8_t(buf, 5, sue_AILERON_NAVIGATION); + _mav_put_uint8_t(buf, 6, sue_RUDDER_NAVIGATION); + _mav_put_uint8_t(buf, 7, sue_ALTITUDEHOLD_STABILIZED); + _mav_put_uint8_t(buf, 8, sue_ALTITUDEHOLD_WAYPOINT); + _mav_put_uint8_t(buf, 9, sue_RACING_MODE); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); +#endif +#else + mavlink_serial_udb_extra_f4_t *packet = (mavlink_serial_udb_extra_f4_t *)msgbuf; + packet->sue_ROLL_STABILIZATION_AILERONS = sue_ROLL_STABILIZATION_AILERONS; + packet->sue_ROLL_STABILIZATION_RUDDER = sue_ROLL_STABILIZATION_RUDDER; + packet->sue_PITCH_STABILIZATION = sue_PITCH_STABILIZATION; + packet->sue_YAW_STABILIZATION_RUDDER = sue_YAW_STABILIZATION_RUDDER; + packet->sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; + packet->sue_AILERON_NAVIGATION = sue_AILERON_NAVIGATION; + packet->sue_RUDDER_NAVIGATION = sue_RUDDER_NAVIGATION; + packet->sue_ALTITUDEHOLD_STABILIZED = sue_ALTITUDEHOLD_STABILIZED; + packet->sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT; + packet->sue_RACING_MODE = sue_RACING_MODE; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F4 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h index e115a9b44..a821f65bb 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_serial_udb_extra_f5_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f5_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float sue_YAWKP_AILERON, float sue_YAWKD_AILERON, float sue_ROLLKP, float sue_ROLLKD, float sue_YAW_STABILIZATION_AILERON, float sue_AILERON_BOOST) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, sue_YAWKP_AILERON); + _mav_put_float(buf, 4, sue_YAWKD_AILERON); + _mav_put_float(buf, 8, sue_ROLLKP); + _mav_put_float(buf, 12, sue_ROLLKD); + _mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON); + _mav_put_float(buf, 20, sue_AILERON_BOOST); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); +#endif +#else + mavlink_serial_udb_extra_f5_t *packet = (mavlink_serial_udb_extra_f5_t *)msgbuf; + packet->sue_YAWKP_AILERON = sue_YAWKP_AILERON; + packet->sue_YAWKD_AILERON = sue_YAWKD_AILERON; + packet->sue_ROLLKP = sue_ROLLKP; + packet->sue_ROLLKD = sue_ROLLKD; + packet->sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; + packet->sue_AILERON_BOOST = sue_AILERON_BOOST; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F5 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h index 656103d09..0eb24c986 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_serial_udb_extra_f6_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f6_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float sue_PITCHGAIN, float sue_PITCHKD, float sue_RUDDER_ELEV_MIX, float sue_ROLL_ELEV_MIX, float sue_ELEVATOR_BOOST) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, sue_PITCHGAIN); + _mav_put_float(buf, 4, sue_PITCHKD); + _mav_put_float(buf, 8, sue_RUDDER_ELEV_MIX); + _mav_put_float(buf, 12, sue_ROLL_ELEV_MIX); + _mav_put_float(buf, 16, sue_ELEVATOR_BOOST); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); +#endif +#else + mavlink_serial_udb_extra_f6_t *packet = (mavlink_serial_udb_extra_f6_t *)msgbuf; + packet->sue_PITCHGAIN = sue_PITCHGAIN; + packet->sue_PITCHKD = sue_PITCHKD; + packet->sue_RUDDER_ELEV_MIX = sue_RUDDER_ELEV_MIX; + packet->sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX; + packet->sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F6 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h index 51c98e6b7..58de7d01c 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_serial_udb_extra_f7_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f7_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float sue_YAWKP_RUDDER, float sue_YAWKD_RUDDER, float sue_ROLLKP_RUDDER, float sue_ROLLKD_RUDDER, float sue_RUDDER_BOOST, float sue_RTL_PITCH_DOWN) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, sue_YAWKP_RUDDER); + _mav_put_float(buf, 4, sue_YAWKD_RUDDER); + _mav_put_float(buf, 8, sue_ROLLKP_RUDDER); + _mav_put_float(buf, 12, sue_ROLLKD_RUDDER); + _mav_put_float(buf, 16, sue_RUDDER_BOOST); + _mav_put_float(buf, 20, sue_RTL_PITCH_DOWN); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); +#endif +#else + mavlink_serial_udb_extra_f7_t *packet = (mavlink_serial_udb_extra_f7_t *)msgbuf; + packet->sue_YAWKP_RUDDER = sue_YAWKP_RUDDER; + packet->sue_YAWKD_RUDDER = sue_YAWKD_RUDDER; + packet->sue_ROLLKP_RUDDER = sue_ROLLKP_RUDDER; + packet->sue_ROLLKD_RUDDER = sue_ROLLKD_RUDDER; + packet->sue_RUDDER_BOOST = sue_RUDDER_BOOST; + packet->sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F7 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h index 8557a95e2..5cab81655 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_serial_udb_extra_f8_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f8_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float sue_HEIGHT_TARGET_MAX, float sue_HEIGHT_TARGET_MIN, float sue_ALT_HOLD_THROTTLE_MIN, float sue_ALT_HOLD_THROTTLE_MAX, float sue_ALT_HOLD_PITCH_MIN, float sue_ALT_HOLD_PITCH_MAX, float sue_ALT_HOLD_PITCH_HIGH) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, sue_HEIGHT_TARGET_MAX); + _mav_put_float(buf, 4, sue_HEIGHT_TARGET_MIN); + _mav_put_float(buf, 8, sue_ALT_HOLD_THROTTLE_MIN); + _mav_put_float(buf, 12, sue_ALT_HOLD_THROTTLE_MAX); + _mav_put_float(buf, 16, sue_ALT_HOLD_PITCH_MIN); + _mav_put_float(buf, 20, sue_ALT_HOLD_PITCH_MAX); + _mav_put_float(buf, 24, sue_ALT_HOLD_PITCH_HIGH); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); +#endif +#else + mavlink_serial_udb_extra_f8_t *packet = (mavlink_serial_udb_extra_f8_t *)msgbuf; + packet->sue_HEIGHT_TARGET_MAX = sue_HEIGHT_TARGET_MAX; + packet->sue_HEIGHT_TARGET_MIN = sue_HEIGHT_TARGET_MIN; + packet->sue_ALT_HOLD_THROTTLE_MIN = sue_ALT_HOLD_THROTTLE_MIN; + packet->sue_ALT_HOLD_THROTTLE_MAX = sue_ALT_HOLD_THROTTLE_MAX; + packet->sue_ALT_HOLD_PITCH_MIN = sue_ALT_HOLD_PITCH_MIN; + packet->sue_ALT_HOLD_PITCH_MAX = sue_ALT_HOLD_PITCH_MAX; + packet->sue_ALT_HOLD_PITCH_HIGH = sue_ALT_HOLD_PITCH_HIGH; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F8 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/version.h b/mavlink/include/mavlink/v1.0/matrixpilot/version.h index a8be16c42..50cbda6c7 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/version.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:04 2014" +#define MAVLINK_BUILD_DATE "Sun Apr 13 09:38:32 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h index ef5354d5e..f1cac9f2c 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target); + _mav_put_uint8_t(buf, 17, roll_manual); + _mav_put_uint8_t(buf, 18, pitch_manual); + _mav_put_uint8_t(buf, 19, yaw_manual); + _mav_put_uint8_t(buf, 20, thrust_manual); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); +#endif +#else + mavlink_attitude_control_t *packet = (mavlink_attitude_control_t *)msgbuf; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->thrust = thrust; + packet->target = target; + packet->roll_manual = roll_manual; + packet->pitch_manual = pitch_manual; + packet->yaw_manual = yaw_manual; + packet->thrust_manual = thrust_manual; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ATTITUDE_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h index d41093792..7bc27982a 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h @@ -217,6 +217,50 @@ static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float #endif } +#if MAVLINK_MSG_ID_BRIEF_FEATURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_brief_feature_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t *descriptor, float response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, response); + _mav_put_uint16_t(buf, 16, size); + _mav_put_uint16_t(buf, 18, orientation); + _mav_put_uint8_t(buf, 20, orientation_assignment); + _mav_put_uint8_t_array(buf, 21, descriptor, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); +#endif +#else + mavlink_brief_feature_t *packet = (mavlink_brief_feature_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->response = response; + packet->size = size; + packet->orientation = orientation; + packet->orientation_assignment = orientation_assignment; + mav_array_memcpy(packet->descriptor, descriptor, sizeof(uint8_t)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE BRIEF_FEATURE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h index ae4db825d..37270d49c 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h @@ -388,6 +388,82 @@ static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_image_available_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, cam_id); + _mav_put_uint64_t(buf, 8, timestamp); + _mav_put_uint64_t(buf, 16, valid_until); + _mav_put_uint32_t(buf, 24, img_seq); + _mav_put_uint32_t(buf, 28, img_buf_index); + _mav_put_uint32_t(buf, 32, key); + _mav_put_uint32_t(buf, 36, exposure); + _mav_put_float(buf, 40, gain); + _mav_put_float(buf, 44, roll); + _mav_put_float(buf, 48, pitch); + _mav_put_float(buf, 52, yaw); + _mav_put_float(buf, 56, local_z); + _mav_put_float(buf, 60, lat); + _mav_put_float(buf, 64, lon); + _mav_put_float(buf, 68, alt); + _mav_put_float(buf, 72, ground_x); + _mav_put_float(buf, 76, ground_y); + _mav_put_float(buf, 80, ground_z); + _mav_put_uint16_t(buf, 84, width); + _mav_put_uint16_t(buf, 86, height); + _mav_put_uint16_t(buf, 88, depth); + _mav_put_uint8_t(buf, 90, cam_no); + _mav_put_uint8_t(buf, 91, channels); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); +#endif +#else + mavlink_image_available_t *packet = (mavlink_image_available_t *)msgbuf; + packet->cam_id = cam_id; + packet->timestamp = timestamp; + packet->valid_until = valid_until; + packet->img_seq = img_seq; + packet->img_buf_index = img_buf_index; + packet->key = key; + packet->exposure = exposure; + packet->gain = gain; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->local_z = local_z; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->ground_x = ground_x; + packet->ground_y = ground_y; + packet->ground_z = ground_z; + packet->width = width; + packet->height = height; + packet->depth = depth; + packet->cam_no = cam_no; + packet->channels = channels; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE IMAGE_AVAILABLE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h index 664574be9..85f8eff55 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h @@ -146,6 +146,38 @@ static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_image_trigger_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t enable) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, enable); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); +#endif +#else + mavlink_image_trigger_control_t *packet = (mavlink_image_trigger_control_t *)msgbuf; + packet->enable = enable; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE IMAGE_TRIGGER_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h index f3a2243af..b438c74ea 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h @@ -267,6 +267,60 @@ static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_image_triggered_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_float(buf, 12, roll); + _mav_put_float(buf, 16, pitch); + _mav_put_float(buf, 20, yaw); + _mav_put_float(buf, 24, local_z); + _mav_put_float(buf, 28, lat); + _mav_put_float(buf, 32, lon); + _mav_put_float(buf, 36, alt); + _mav_put_float(buf, 40, ground_x); + _mav_put_float(buf, 44, ground_y); + _mav_put_float(buf, 48, ground_z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); +#endif +#else + mavlink_image_triggered_t *packet = (mavlink_image_triggered_t *)msgbuf; + packet->timestamp = timestamp; + packet->seq = seq; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->local_z = local_z; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->ground_x = ground_x; + packet->ground_y = ground_y; + packet->ground_z = ground_z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE IMAGE_TRIGGERED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h index 6bdcceaf9..05452637e 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, #endif } +#if MAVLINK_MSG_ID_MARKER_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_marker_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, roll); + _mav_put_float(buf, 16, pitch); + _mav_put_float(buf, 20, yaw); + _mav_put_uint16_t(buf, 24, id); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, MAVLINK_MSG_ID_MARKER_LEN); +#endif +#else + mavlink_marker_t *packet = (mavlink_marker_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->id = id; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)packet, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)packet, MAVLINK_MSG_ID_MARKER_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MARKER UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h index aa7b827a3..bde400da9 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_PATTERN_DETECTED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_pattern_detected_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, float confidence, const char *file, uint8_t detected) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, confidence); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 105, detected); + _mav_put_char_array(buf, 5, file, 100); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); +#endif +#else + mavlink_pattern_detected_t *packet = (mavlink_pattern_detected_t *)msgbuf; + packet->confidence = confidence; + packet->type = type; + packet->detected = detected; + mav_array_memcpy(packet->file, file, sizeof(char)*100); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PATTERN_DETECTED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h index 913a52897..86e934519 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h @@ -217,6 +217,50 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_point_of_interest_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_uint16_t(buf, 12, timeout); + _mav_put_uint8_t(buf, 14, type); + _mav_put_uint8_t(buf, 15, color); + _mav_put_uint8_t(buf, 16, coordinate_system); + _mav_put_char_array(buf, 17, name, 26); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); +#endif +#else + mavlink_point_of_interest_t *packet = (mavlink_point_of_interest_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->timeout = timeout; + packet->type = type; + packet->color = color; + packet->coordinate_system = coordinate_system; + mav_array_memcpy(packet->name, name, sizeof(char)*26); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE POINT_OF_INTEREST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h index e24436431..e5c04a851 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h @@ -250,6 +250,56 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel #endif } +#if MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_point_of_interest_connection_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, xp1); + _mav_put_float(buf, 4, yp1); + _mav_put_float(buf, 8, zp1); + _mav_put_float(buf, 12, xp2); + _mav_put_float(buf, 16, yp2); + _mav_put_float(buf, 20, zp2); + _mav_put_uint16_t(buf, 24, timeout); + _mav_put_uint8_t(buf, 26, type); + _mav_put_uint8_t(buf, 27, color); + _mav_put_uint8_t(buf, 28, coordinate_system); + _mav_put_char_array(buf, 29, name, 26); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); +#endif +#else + mavlink_point_of_interest_connection_t *packet = (mavlink_point_of_interest_connection_t *)msgbuf; + packet->xp1 = xp1; + packet->yp1 = yp1; + packet->zp1 = zp1; + packet->xp2 = xp2; + packet->yp2 = yp2; + packet->zp2 = zp2; + packet->timeout = timeout; + packet->type = type; + packet->color = color; + packet->coordinate_system = coordinate_system; + mav_array_memcpy(packet->name, name, sizeof(char)*26); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE POINT_OF_INTEREST_CONNECTION UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h index 6f4ca510a..480004cef 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t #endif } +#if MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_position_control_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, float x, float y, float z, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint16_t(buf, 16, id); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); +#endif +#else + mavlink_position_control_setpoint_t *packet = (mavlink_position_control_setpoint_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->yaw = yaw; + packet->id = id; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE POSITION_CONTROL_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h index 08cedbb4b..41a512648 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc #endif } +#if MAVLINK_MSG_ID_RAW_AUX_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_raw_aux_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, baro); + _mav_put_uint16_t(buf, 4, adc1); + _mav_put_uint16_t(buf, 6, adc2); + _mav_put_uint16_t(buf, 8, adc3); + _mav_put_uint16_t(buf, 10, adc4); + _mav_put_uint16_t(buf, 12, vbat); + _mav_put_int16_t(buf, 14, temp); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, MAVLINK_MSG_ID_RAW_AUX_LEN); +#endif +#else + mavlink_raw_aux_t *packet = (mavlink_raw_aux_t *)msgbuf; + packet->baro = baro; + packet->adc1 = adc1; + packet->adc2 = adc2; + packet->adc3 = adc3; + packet->adc4 = adc4; + packet->vbat = vbat; + packet->temp = temp; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)packet, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)packet, MAVLINK_MSG_ID_RAW_AUX_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RAW_AUX UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h index b26d748c2..8fe14dd54 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_cam_shutter_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, gain); + _mav_put_uint16_t(buf, 4, interval); + _mav_put_uint16_t(buf, 6, exposure); + _mav_put_uint8_t(buf, 8, cam_no); + _mav_put_uint8_t(buf, 9, cam_mode); + _mav_put_uint8_t(buf, 10, trigger_pin); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); +#endif +#else + mavlink_set_cam_shutter_t *packet = (mavlink_set_cam_shutter_t *)msgbuf; + packet->gain = gain; + packet->interval = interval; + packet->exposure = exposure; + packet->cam_no = cam_no; + packet->cam_mode = cam_mode; + packet->trigger_pin = trigger_pin; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_CAM_SHUTTER UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h index f1f9e698f..c4e4fc19a 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_set_position_control_offset_send(mavlink_channel_ #endif } +#if MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_position_control_offset_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); +#endif +#else + mavlink_set_position_control_offset_t *packet = (mavlink_set_position_control_offset_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->yaw = yaw; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_POSITION_CONTROL_OFFSET UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h index 9458087da..d71a2ed2e 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_watchdog_command_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, watchdog_id); + _mav_put_uint16_t(buf, 2, process_id); + _mav_put_uint8_t(buf, 4, target_system_id); + _mav_put_uint8_t(buf, 5, command_id); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); +#endif +#else + mavlink_watchdog_command_t *packet = (mavlink_watchdog_command_t *)msgbuf; + packet->watchdog_id = watchdog_id; + packet->process_id = process_id; + packet->target_system_id = target_system_id; + packet->command_id = command_id; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE WATCHDOG_COMMAND UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h index 3f4295ee5..a8e49b642 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, u #endif } +#if MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_watchdog_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, watchdog_id); + _mav_put_uint16_t(buf, 2, process_count); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); +#endif +#else + mavlink_watchdog_heartbeat_t *packet = (mavlink_watchdog_heartbeat_t *)msgbuf; + packet->watchdog_id = watchdog_id; + packet->process_count = process_count; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE WATCHDOG_HEARTBEAT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h index 55853cdde..a5e0cc67e 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h @@ -185,6 +185,44 @@ static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_watchdog_process_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char *name, const char *arguments, int32_t timeout) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, timeout); + _mav_put_uint16_t(buf, 4, watchdog_id); + _mav_put_uint16_t(buf, 6, process_id); + _mav_put_char_array(buf, 8, name, 100); + _mav_put_char_array(buf, 108, arguments, 147); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); +#endif +#else + mavlink_watchdog_process_info_t *packet = (mavlink_watchdog_process_info_t *)msgbuf; + packet->timeout = timeout; + packet->watchdog_id = watchdog_id; + packet->process_id = process_id; + mav_array_memcpy(packet->name, name, sizeof(char)*100); + mav_array_memcpy(packet->arguments, arguments, sizeof(char)*147); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE WATCHDOG_PROCESS_INFO UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h index a0410d803..15c9598e4 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_watchdog_process_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, pid); + _mav_put_uint16_t(buf, 4, watchdog_id); + _mav_put_uint16_t(buf, 6, process_id); + _mav_put_uint16_t(buf, 8, crashes); + _mav_put_uint8_t(buf, 10, state); + _mav_put_uint8_t(buf, 11, muted); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); +#endif +#else + mavlink_watchdog_process_status_t *packet = (mavlink_watchdog_process_status_t *)msgbuf; + packet->pid = pid; + packet->watchdog_id = watchdog_id; + packet->process_id = process_id; + packet->crashes = crashes; + packet->state = state; + packet->muted = muted; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE WATCHDOG_PROCESS_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h index 428619eed..a624a2579 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 0, 0, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 0, 0, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 108, 86, 95, 224, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 0, 0, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 108, 86, 95, 224, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 0, 0, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_POSITION_CONTROL_OFFSET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_POSITION_CONTROL_OFFSET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -56,6 +56,7 @@ enum MAV_CMD MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ @@ -73,8 +74,16 @@ enum MAV_CMD MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ diff --git a/mavlink/include/mavlink/v1.0/pixhawk/version.h b/mavlink/include/mavlink/v1.0/pixhawk/version.h index aa79f6ffb..6610a894c 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/version.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:16 2014" +#define MAVLINK_BUILD_DATE "Sun Apr 13 09:38:46 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h index 73e9d75db..a87ce87d3 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h @@ -173,6 +173,40 @@ static inline void mavlink_msg_cmd_airspeed_ack_send(mavlink_channel_t chan, flo #endif } +#if MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_cmd_airspeed_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float spCmd, uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, spCmd); + _mav_put_uint8_t(buf, 4, ack); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); +#endif +#else + mavlink_cmd_airspeed_ack_t *packet = (mavlink_cmd_airspeed_ack_t *)msgbuf; + packet->spCmd = spCmd; + packet->ack = ack; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE CMD_AIRSPEED_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h index b6b567f99..71d61e1e0 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h @@ -173,6 +173,40 @@ static inline void mavlink_msg_cmd_airspeed_chng_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_cmd_airspeed_chng_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, float spCmd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, spCmd); + _mav_put_uint8_t(buf, 4, target); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); +#endif +#else + mavlink_cmd_airspeed_chng_t *packet = (mavlink_cmd_airspeed_chng_t *)msgbuf; + packet->spCmd = spCmd; + packet->target = target; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE CMD_AIRSPEED_CHNG UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h index 642f7aab9..40a3db25a 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h @@ -154,6 +154,38 @@ static inline void mavlink_msg_filt_rot_vel_send(mavlink_channel_t chan, const f #endif } +#if MAVLINK_MSG_ID_FILT_ROT_VEL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_filt_rot_vel_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const float *rotVel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_float_array(buf, 0, rotVel, 3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); +#endif +#else + mavlink_filt_rot_vel_t *packet = (mavlink_filt_rot_vel_t *)msgbuf; + + mav_array_memcpy(packet->rotVel, rotVel, sizeof(float)*3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FILT_ROT_VEL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h index 0f8369881..bceed1013 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h @@ -174,6 +174,40 @@ static inline void mavlink_msg_llc_out_send(mavlink_channel_t chan, const int16_ #endif } +#if MAVLINK_MSG_ID_LLC_OUT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_llc_out_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const int16_t *servoOut, const int16_t *MotorOut) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_int16_t_array(buf, 0, servoOut, 4); + _mav_put_int16_t_array(buf, 8, MotorOut, 2); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, MAVLINK_MSG_ID_LLC_OUT_LEN); +#endif +#else + mavlink_llc_out_t *packet = (mavlink_llc_out_t *)msgbuf; + + mav_array_memcpy(packet->servoOut, servoOut, sizeof(int16_t)*4); + mav_array_memcpy(packet->MotorOut, MotorOut, sizeof(int16_t)*2); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)packet, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)packet, MAVLINK_MSG_ID_LLC_OUT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LLC_OUT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h index 5d9382326..f5a43e4fa 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h @@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_air_temp_send(mavlink_channel_t chan, float a #endif } +#if MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_air_temp_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float airT) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, airT); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); +#endif +#else + mavlink_obs_air_temp_t *packet = (mavlink_obs_air_temp_t *)msgbuf; + packet->airT = airT; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_AIR_TEMP UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h index 35d813ca1..67d5ab2a2 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h @@ -192,6 +192,42 @@ static inline void mavlink_msg_obs_air_velocity_send(mavlink_channel_t chan, flo #endif } +#if MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_air_velocity_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float magnitude, float aoa, float slip) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, magnitude); + _mav_put_float(buf, 4, aoa); + _mav_put_float(buf, 8, slip); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); +#endif +#else + mavlink_obs_air_velocity_t *packet = (mavlink_obs_air_velocity_t *)msgbuf; + packet->magnitude = magnitude; + packet->aoa = aoa; + packet->slip = slip; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_AIR_VELOCITY UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h index 9c80cd66e..e1178e25f 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h @@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_attitude_send(mavlink_channel_t chan, const d #endif } +#if MAVLINK_MSG_ID_OBS_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const double *quat) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_double_array(buf, 0, quat, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); +#endif +#else + mavlink_obs_attitude_t *packet = (mavlink_obs_attitude_t *)msgbuf; + + mav_array_memcpy(packet->quat, quat, sizeof(double)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_ATTITUDE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h index 24dd43b57..b164687ff 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h @@ -174,6 +174,40 @@ static inline void mavlink_msg_obs_bias_send(mavlink_channel_t chan, const float #endif } +#if MAVLINK_MSG_ID_OBS_BIAS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_bias_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const float *accBias, const float *gyroBias) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_float_array(buf, 0, accBias, 3); + _mav_put_float_array(buf, 12, gyroBias, 3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, MAVLINK_MSG_ID_OBS_BIAS_LEN); +#endif +#else + mavlink_obs_bias_t *packet = (mavlink_obs_bias_t *)msgbuf; + + mav_array_memcpy(packet->accBias, accBias, sizeof(float)*3); + mav_array_memcpy(packet->gyroBias, gyroBias, sizeof(float)*3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)packet, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)packet, MAVLINK_MSG_ID_OBS_BIAS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_BIAS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h index cfc2fe7e1..f0ecef0e7 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h @@ -192,6 +192,42 @@ static inline void mavlink_msg_obs_position_send(mavlink_channel_t chan, int32_t #endif } +#if MAVLINK_MSG_ID_OBS_POSITION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lon, int32_t lat, int32_t alt) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lon); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, alt); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, MAVLINK_MSG_ID_OBS_POSITION_LEN); +#endif +#else + mavlink_obs_position_t *packet = (mavlink_obs_position_t *)msgbuf; + packet->lon = lon; + packet->lat = lat; + packet->alt = alt; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)packet, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)packet, MAVLINK_MSG_ID_OBS_POSITION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_POSITION UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h index 24e272bf7..31617c883 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h @@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_qff_send(mavlink_channel_t chan, float qff) #endif } +#if MAVLINK_MSG_ID_OBS_QFF_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_qff_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float qff) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, qff); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN); +#endif +#else + mavlink_obs_qff_t *packet = (mavlink_obs_qff_t *)msgbuf; + packet->qff = qff; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)packet, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)packet, MAVLINK_MSG_ID_OBS_QFF_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_QFF UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h index 6e3776632..ec815d692 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h @@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_velocity_send(mavlink_channel_t chan, const f #endif } +#if MAVLINK_MSG_ID_OBS_VELOCITY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_velocity_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const float *vel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_float_array(buf, 0, vel, 3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); +#endif +#else + mavlink_obs_velocity_t *packet = (mavlink_obs_velocity_t *)msgbuf; + + mav_array_memcpy(packet->vel, vel, sizeof(float)*3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_VELOCITY UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h index 55f7cb2ae..c40391d44 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h @@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_wind_send(mavlink_channel_t chan, const float #endif } +#if MAVLINK_MSG_ID_OBS_WIND_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_wind_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const float *wind) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_float_array(buf, 0, wind, 3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN); +#endif +#else + mavlink_obs_wind_t *packet = (mavlink_obs_wind_t *)msgbuf; + + mav_array_memcpy(packet->wind, wind, sizeof(float)*3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)packet, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)packet, MAVLINK_MSG_ID_OBS_WIND_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_WIND UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h index e0963ece7..70524f464 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h @@ -186,6 +186,40 @@ static inline void mavlink_msg_pm_elec_send(mavlink_channel_t chan, float PwCons #endif } +#if MAVLINK_MSG_ID_PM_ELEC_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_pm_elec_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float PwCons, float BatStat, const float *PwGen) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, PwCons); + _mav_put_float(buf, 4, BatStat); + _mav_put_float_array(buf, 8, PwGen, 3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, MAVLINK_MSG_ID_PM_ELEC_LEN); +#endif +#else + mavlink_pm_elec_t *packet = (mavlink_pm_elec_t *)msgbuf; + packet->PwCons = PwCons; + packet->BatStat = BatStat; + mav_array_memcpy(packet->PwGen, PwGen, sizeof(float)*3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)packet, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)packet, MAVLINK_MSG_ID_PM_ELEC_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PM_ELEC UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h index 94f3e58bb..72f8a8193 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h @@ -211,6 +211,44 @@ static inline void mavlink_msg_sys_stat_send(mavlink_channel_t chan, uint8_t gps #endif } +#if MAVLINK_MSG_ID_SYS_Stat_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sys_stat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t gps, uint8_t act, uint8_t mod, uint8_t commRssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, gps); + _mav_put_uint8_t(buf, 1, act); + _mav_put_uint8_t(buf, 2, mod); + _mav_put_uint8_t(buf, 3, commRssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, MAVLINK_MSG_ID_SYS_Stat_LEN); +#endif +#else + mavlink_sys_stat_t *packet = (mavlink_sys_stat_t *)msgbuf; + packet->gps = gps; + packet->act = act; + packet->mod = mod; + packet->commRssi = commRssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)packet, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)packet, MAVLINK_MSG_ID_SYS_Stat_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SYS_Stat UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h b/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h index 26666b0cc..2eab86354 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_POSITION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_ATTITUDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_WIND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_BIAS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_QFF, MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP, MAVLINK_MESSAGE_INFO_FILT_ROT_VEL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LLC_OUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PM_ELEC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_Stat, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_POSITION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_ATTITUDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_WIND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_BIAS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_QFF, MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP, MAVLINK_MESSAGE_INFO_FILT_ROT_VEL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LLC_OUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PM_ELEC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_Stat, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" diff --git a/mavlink/include/mavlink/v1.0/sensesoar/version.h b/mavlink/include/mavlink/v1.0/sensesoar/version.h index cdd683949..78d562803 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/version.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:27 2014" +#define MAVLINK_BUILD_DATE "Sun Apr 13 09:39:00 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 -- cgit v1.2.3 From e7594a75fb9de2f75c96fa836aef1d956370070d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 14 Apr 2014 21:44:51 +0200 Subject: startup: cleanup of cox mixer files (Thanks Rune) --- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 2 +- ROMFS/px4fmu_common/init.d/12001_octo_cox | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix | 3 +++ ROMFS/px4fmu_common/mixers/hexa_cox.mix | 3 --- 5 files changed, 6 insertions(+), 6 deletions(-) create mode 100644 ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix delete mode 100644 ROMFS/px4fmu_common/mixers/hexa_cox.mix diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index 38e65435b..df2e609bc 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -9,7 +9,7 @@ sh /etc/init.d/rc.mc_defaults -set MIXER hexa_cox +set MIXER FMU_hexa_cox # We only can run one channel group with one rate, so set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 99f902a6d..8703f5f2f 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -7,6 +7,6 @@ sh /etc/init.d/rc.mc_defaults -set MIXER octo_cox +set MIXER FMU_octo_cox set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0cb6d281a..2ab1e2e96 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -514,7 +514,7 @@ then then set MAV_TYPE 13 fi - if [ $MIXER == hexa_cox ] + if [ $MIXER == FMU_hexa_cox ] then set MAV_TYPE 13 fi diff --git a/ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix new file mode 100644 index 000000000..497786feb --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix @@ -0,0 +1,3 @@ +# Hexa coaxial + +R: 6c 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/hexa_cox.mix b/ROMFS/px4fmu_common/mixers/hexa_cox.mix deleted file mode 100644 index 497786feb..000000000 --- a/ROMFS/px4fmu_common/mixers/hexa_cox.mix +++ /dev/null @@ -1,3 +0,0 @@ -# Hexa coaxial - -R: 6c 10000 10000 10000 0 -- cgit v1.2.3 From 654ab4635ad0fb70d6c0cb601d56e3d97691bdac Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 18 Apr 2014 11:15:40 +0200 Subject: navigator: wrong mission topic was copied, clearer naming of offboard mission now --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- src/modules/navigator/navigator_main.cpp | 6 +++--- src/modules/uORB/objects_common.cpp | 2 +- src/modules/uORB/topics/mission.h | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 072f16aae..36d47b7ee 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -832,10 +832,10 @@ void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(mission), &mission); + _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); } else { - orb_publish(ORB_ID(mission), _mission_pub, &mission); + orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission); } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1bb5d33a1..494266dd3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -510,7 +510,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; - if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ @@ -543,7 +543,7 @@ Navigator::onboard_mission_update() { struct mission_s onboard_mission; - if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_onboard_mission_count(onboard_mission.count); _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -611,7 +611,7 @@ Navigator::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index c8a589bb7..90675bb2e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -127,7 +127,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); #include "topics/mission.h" -ORB_DEFINE(mission, struct mission_s); +ORB_DEFINE(offboard_mission, struct mission_s); ORB_DEFINE(onboard_mission, struct mission_s); #include "topics/mission_result.h" diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 1ccdb7181..dfc461ae4 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -107,7 +107,7 @@ struct mission_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission); +ORB_DECLARE(offboard_mission); ORB_DECLARE(onboard_mission); #endif -- cgit v1.2.3 From 671c7a115a7c01ed89266a6631fb3929af84ffcf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 21 Apr 2014 16:20:20 +0200 Subject: simple underspeed protection for mtecs --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 27 ++++++++++----- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 10 ++++-- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 40 +++++++++++++++++++++- 3 files changed, 65 insertions(+), 12 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 63e60995e..1fb3153e9 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -50,6 +50,7 @@ mTecs::mTecs() : SuperBlock(NULL, "MT"), /* Parameters */ _mTecsEnabled(this, "ENABLED"), + _airspeedMin(this, "FW_AIRSPD_MIN", false), /* control blocks */ _controlTotalEnergy(this, "THR"), _controlEnergyDistribution(this, "PIT", true), @@ -58,11 +59,13 @@ mTecs::mTecs() : _airspeedDerivative(this, "AD"), _throttleSp(0.0f), _pitchSp(0.0f), - BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"), - BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), + _BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"), + _BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), + _BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"), + _BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true), timestampLastIteration(hrt_absolute_time()), _firstIterationAfterReset(true), - dtCalculated(false), + _dtCalculated(false), _counter(0) { } @@ -133,12 +136,20 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); } + /* Check airspeed: if below safe value switch to underspeed mode */ + if (airspeed < _airspeedMin.get()) { + mode = TECS_MODE_UNDERSPEED; + } + /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits if (mode == TECS_MODE_TAKEOFF) { - outputLimiterThrottle = &BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector - outputLimiterPitch = &BlockOutputLimiterTakeoffPitch; + outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector + outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; + } else if (mode == TECS_MODE_UNDERSPEED) { + outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; + outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; } /** update control blocks **/ @@ -159,7 +170,7 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh /* clean up */ _firstIterationAfterReset = false; - dtCalculated = false; + _dtCalculated = false; _counter++; } @@ -174,7 +185,7 @@ void mTecs::resetIntegrators() void mTecs::updateTimeMeasurement() { - if (!dtCalculated) { + if (!_dtCalculated) { float deltaTSeconds = 0.0f; if (!_firstIterationAfterReset) { hrt_abstime timestampNow = hrt_absolute_time(); @@ -183,7 +194,7 @@ void mTecs::updateTimeMeasurement() } setDt(deltaTSeconds); - dtCalculated = true; + _dtCalculated = true; } } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 6c30a458a..393dd294d 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -60,6 +60,7 @@ public: typedef enum { TECS_MODE_NORMAL, + TECS_MODE_UNDERSPEED, TECS_MODE_TAKEOFF } tecs_mode; @@ -92,6 +93,7 @@ public: protected: /* parameters */ control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ + control::BlockParamFloat _airspeedMin; /**< minimal airspeed */ /* control blocks */ BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ @@ -107,14 +109,16 @@ protected: float _pitchSp; /**< Pitch Setpoint from -pi to pi */ /* Output Limits in special modes */ - BlockOutputLimiter BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ - BlockOutputLimiter BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit during takeoff */ /* Time measurements */ hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ - bool dtCalculated; /**< True if dt has been calculated in this iteration */ + bool _dtCalculated; /**< True if dt has been calculated in this iteration */ int _counter; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 85a4f3d48..fd501c17a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -247,7 +247,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); */ PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); - /** * Minimal throttle during takeoff * @@ -285,3 +284,42 @@ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f); * @group mTECS */ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f); + +/** + * Minimal throttle in underspeed mode + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f); + +/** + * Maximal throttle in underspeed mode + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f); + +/** + * Minimal pitch in underspeed mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f); + +/** + * Maximal pitch in underspeed mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f); + -- cgit v1.2.3 From 488785250f4f1fa3c2f6d1e3283fd8eabb6b3144 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 21 Apr 2014 17:35:42 +0200 Subject: fw_att_pos_estimator: added simple in-air/on-ground detector --- .../fw_att_pos_estimator_main.cpp | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 840cd585e..a61fff942 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -211,6 +211,9 @@ private: AttPosEKF *_ekf; + float _velocity_xy_filtered; + float _velocity_z_filtered; + /** * Update our local parameter cache. */ @@ -292,7 +295,9 @@ FixedwingEstimator::FixedwingEstimator() : _initialized(false), _gps_initialized(false), _mavlink_fd(-1), - _ekf(nullptr) + _ekf(nullptr), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f) { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -1030,6 +1035,21 @@ FixedwingEstimator::task_main() _local_pos.z_global = false; _local_pos.yaw = _att.yaw; + // _velocity_xy_filtered = 0.9f*_velocity_xy_filtered + 0.1f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); + // _velocity_z_filtered = 0.9f*_velocity_z_filtered + 0.1f*fabsf(_local_pos.vz); + + + /* crude land detector for fixedwing only, + * TODO: adapt so that it works for both, maybe move to another location + */ + if (_velocity_xy_filtered < 2 + && _velocity_z_filtered < 2 + && _airspeed.true_airspeed_m_s < 10) { + _local_pos.landed = true; + } else { + _local_pos.landed = false; + } + /* lazily publish the local position only once available */ if (_local_pos_pub > 0) { /* publish the attitude setpoint */ -- cgit v1.2.3 From c3c0328e8bb9211580dbe5a52ecb23e0452cb402 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 21 Apr 2014 17:36:59 +0200 Subject: navigator: lot's of cleanup (WIP) --- src/modules/commander/commander.cpp | 57 +- src/modules/commander/state_machine_helper.cpp | 6 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 +- src/modules/mavlink/mavlink_main.cpp | 7 +- src/modules/mavlink/mavlink_messages.cpp | 5 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- src/modules/navigator/geofence_params.c | 1 - src/modules/navigator/mission.cpp | 297 +++++++ src/modules/navigator/mission.h | 98 +++ src/modules/navigator/module.mk | 6 +- src/modules/navigator/navigator_main.cpp | 934 +++++++-------------- src/modules/navigator/navigator_mission.cpp | 332 -------- src/modules/navigator/navigator_mission.h | 106 --- src/modules/navigator/navigator_params.c | 41 - src/modules/navigator/navigator_state.h | 21 - src/modules/navigator/rtl.cpp | 253 ++++++ src/modules/navigator/rtl.h | 92 ++ src/modules/navigator/rtl_params.c | 87 ++ src/modules/systemlib/state_table.h | 23 +- src/modules/uORB/topics/mission.h | 2 + .../uORB/topics/position_setpoint_triplet.h | 12 +- src/modules/uORB/topics/vehicle_global_position.h | 12 +- src/modules/uORB/topics/vehicle_status.h | 13 +- 23 files changed, 1221 insertions(+), 1194 deletions(-) create mode 100644 src/modules/navigator/mission.cpp create mode 100644 src/modules/navigator/mission.h delete mode 100644 src/modules/navigator/navigator_mission.cpp delete mode 100644 src/modules/navigator/navigator_mission.h delete mode 100644 src/modules/navigator/navigator_state.h create mode 100644 src/modules/navigator/rtl.cpp create mode 100644 src/modules/navigator/rtl.h create mode 100644 src/modules/navigator/rtl_params.c diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e7cf2b3fa..c2d24a883 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -547,15 +547,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAV_STATE_LOITER; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAV_STATE_MISSION; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -668,8 +666,7 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAV_STATE_NONE; - status.set_nav_state_timestamp = 0; + status.set_nav_state = NAVIGATION_STATE_NONE; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; status.failsafe_state = FAILSAFE_STATE_NORMAL; @@ -944,7 +941,7 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); static bool published_condition_landed_fw = false; - if (status.is_rotary_wing && status.condition_local_altitude_valid) { + if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; @@ -1222,30 +1219,30 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } - /* set navigation state */ - /* RETURN switch, overrides MISSION switch */ - if (sp_man.return_switch == SWITCH_POS_ON) { - /* switch to RTL if not already landed after RTL and home position set */ - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); + if (sp_man.mode_switch == SWITCH_POS_ON) { - } else { - /* MISSION switch */ - if (sp_man.mission_switch == SWITCH_POS_ON) { - /* stick is in LOITER position */ - status.set_nav_state = NAV_STATE_LOITER; - status.set_nav_state_timestamp = hrt_absolute_time(); - - } else if (sp_man.mission_switch != SWITCH_POS_NONE) { - /* stick is in MISSION position */ - status.set_nav_state = NAV_STATE_MISSION; - status.set_nav_state_timestamp = hrt_absolute_time(); - - } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && - pos_sp_triplet.nav_state == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - status.set_nav_state = NAV_STATE_MISSION; - status.set_nav_state_timestamp = hrt_absolute_time(); + /* set navigation state */ + /* RETURN switch, overrides MISSION switch */ + if (sp_man.return_switch == SWITCH_POS_ON) { + /* switch to RTL if not already landed after RTL and home position set */ + status.set_nav_state = NAVIGATION_STATE_RTL; + + } else { + /* MISSION switch */ + if (sp_man.mission_switch == SWITCH_POS_ON) { + /* stick is in LOITER position */ + status.set_nav_state = NAVIGATION_STATE_LOITER; + + } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + /* stick is in MISSION position */ + status.set_nav_state = NAVIGATION_STATE_MISSION; + } + /* XXX: I don't understand this */ + //else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && + // pos_sp_triplet.nav_state == NAVIGATION_STATE_RTL) { + // /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + // status.set_nav_state = NAV_STATE_MISSION; + // } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e6f245cf9..54f0f13f4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -450,8 +450,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->set_nav_state = NAV_STATE_RTL; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_RTL; ret = TRANSITION_CHANGED; } @@ -461,8 +460,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { - status->set_nav_state = NAV_STATE_LAND; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_LAND; ret = TRANSITION_CHANGED; } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f13df785..a00a388a8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -150,8 +150,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - /** manual control states */ float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ double _loiter_hold_lat; @@ -393,7 +391,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), /* states */ - _setpoint_valid(false), _loiter_hold(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), @@ -663,7 +660,6 @@ FixedwingPositionControl::vehicle_setpoint_poll() if (pos_sp_triplet_updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - _setpoint_valid = true; } } @@ -708,7 +704,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { + if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); @@ -781,7 +777,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX this should only execute if auto AND safety off (actuators active), // else integrators should be constantly reset. - if (_control_mode.flag_control_position_enabled) { + if (pos_sp_triplet.current.valid) { /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 36d47b7ee..c60c85495 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -872,7 +872,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item break; case MAV_CMD_DO_JUMP: mission_item->do_jump_mission_index = mavlink_mission_item->param1; - /* TODO: also save param2 (repeat count) */ + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; break; default: mission_item->acceptance_radius = mavlink_mission_item->param2; @@ -889,6 +889,9 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; + /* reset DO_JUMP count */ + mission_item->do_jump_current_count = 0; + return OK; } @@ -908,7 +911,7 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ case NAV_CMD_DO_JUMP: mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - /* TODO: also save param2 (repeat count) */ + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; break; default: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 648228e3b..3cf69bf7c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -117,7 +117,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { + if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND + || pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) { /* use main state when navigator is not active */ if (status->main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); @@ -142,7 +143,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (pos_sp_triplet->nav_state == NAV_STATE_READY) { + if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2bd2d356a..7d3d39d18 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -645,7 +645,7 @@ MulticopterPositionControl::task_main() _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = true; - _pos_sp_triplet.nav_state = NAV_STATE_NONE; + // _pos_sp_triplet.nav_state = NAV_STATE_NONE; _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; _pos_sp_triplet.current.lat = _lat_sp; _pos_sp_triplet.current.lon = _lon_sp; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 5831a0ca9..653b1ad84 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp new file mode 100644 index 000000000..e95bcfd43 --- /dev/null +++ b/src/modules/navigator/mission.cpp @@ -0,0 +1,297 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.cpp + * + * Helper class to access missions + * + * @author Julian Oes + */ + +#include +#include + +#include +#include + +#include +#include + +#include "mission.h" + + +Mission::Mission() : + + _offboard_dataman_id(-1), + _current_offboard_mission_index(0), + _current_onboard_mission_index(0), + _offboard_mission_item_count(0), + _onboard_mission_item_count(0), + _onboard_mission_allowed(false), + _current_mission_type(MISSION_TYPE_NONE), + _mission_result_pub(-1), + _mission_result({}) +{ +} + +Mission::~Mission() +{ +} + +void +Mission::set_offboard_dataman_id(const int new_id) +{ + _offboard_dataman_id = new_id; +} + +void +Mission::set_offboard_mission_count(int new_count) +{ + _offboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_count(int new_count) +{ + _onboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_allowed(bool allowed) +{ + _onboard_mission_allowed = allowed; +} + +bool +Mission::command_current_offboard_mission_index(const int new_index) +{ + if (new_index >= 0) { + _current_offboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_offboard_mission_index >= _offboard_mission_item_count) { + _current_offboard_mission_index = 0; + } + } + report_current_offboard_mission_item(); +} + +bool +Mission::command_current_onboard_mission_index(int new_index) +{ + if (new_index != -1) { + _current_onboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_onboard_mission_index >= _onboard_mission_item_count) { + _current_onboard_mission_index = 0; + } + } + // TODO: implement this for onboard missions as well + // report_current_mission_item(); +} + +bool +Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index) +{ + *onboard = false; + *index = -1; + + /* try onboard mission first */ + if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) { + _current_mission_type = MISSION_TYPE_ONBOARD; + *onboard = true; + *index = _current_onboard_mission_index; + + return true; + } + } + + /* otherwise fallback to offboard */ + if (_current_offboard_mission_index < _offboard_mission_item_count) { + + dm_item_t dm_current; + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) { + + _current_mission_type = MISSION_TYPE_OFFBOARD; + *onboard = false; + *index = _current_offboard_mission_index; + + return true; + } + } + + /* happens when no more mission items can be added as a next item */ + _current_mission_type = MISSION_TYPE_NONE; + + return false; +} + +bool +Mission::get_next_mission_item(struct mission_item_s *new_mission_item) +{ + int next_temp_mission_index = _current_onboard_mission_index + 1; + + /* try onboard mission first */ + if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) { + return true; + } + } + + /* then try offboard mission */ + dm_item_t dm_current; + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + next_temp_mission_index = _current_offboard_mission_index + 1; + if (next_temp_mission_index < _offboard_mission_item_count) { + if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) { + return true; + } + } + + /* both failed, bail out */ + return false; +} + +bool +Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item) +{ + /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + for (int i=0; i<10; i++) { + const ssize_t len = sizeof(struct mission_item_s); + + /* read mission item from datamanager */ + if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ + if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + + if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + + /* only raise the repeat count if this is for the current mission item + * but not for the next mission item */ + if (is_current) { + (new_mission_item->do_jump_current_count)++; + + /* save repeat count */ + if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + } + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; + } else { + return false; + } + + } else { + /* if it's not a DO_JUMP, then we were successful */ + return true; + } + } + + /* we have given up, we don't want to cycle forever */ + warnx("ERROR: cycling through mission items without success"); + return false; +} + +void +Mission::move_to_next() +{ + report_mission_item_reached(); + + switch (_current_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; + } +} + +void +Mission::report_mission_item_reached() +{ + if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + _mission_result.mission_reached = true; + _mission_result.mission_index_reached = _current_offboard_mission_index; + } + publish_mission_result(); +} + +void +Mission::report_current_offboard_mission_item() +{ + _mission_result.index_current_mission = _current_offboard_mission_index; + publish_mission_result(); +} + +void +Mission::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); + + } else { + /* advertise and publish */ + _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + } + /* reset reached bool */ + _mission_result.mission_reached = false; +} + diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h new file mode 100644 index 000000000..0fa2ff3fa --- /dev/null +++ b/src/modules/navigator/mission.h @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.h + * Helper class to access missions + * @author Julian Oes + */ + +#ifndef NAVIGATOR_MISSION_H +#define NAVIGATOR_MISSION_H + +#include +#include +#include + + +class __EXPORT Mission +{ +public: + /** + * Constructor + */ + Mission(); + + /** + * Destructor + */ + ~Mission(); + + void set_offboard_dataman_id(const int new_id); + void set_offboard_mission_count(const int new_count); + void set_onboard_mission_count(const int new_count); + void set_onboard_mission_allowed(const bool allowed); + + bool command_current_offboard_mission_index(const int new_index); + bool command_current_onboard_mission_index(const int new_index); + + bool get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index); + bool get_next_mission_item(struct mission_item_s *mission_item); + + void move_to_next(); + +private: + bool read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item); + + void report_mission_item_reached(); + void report_current_offboard_mission_item(); + + void publish_mission_result(); + + int _offboard_dataman_id; + int _current_offboard_mission_index; + int _current_onboard_mission_index; + int _offboard_mission_item_count; /** number of offboard mission items available */ + int _onboard_mission_item_count; /** number of onboard mission items available */ + bool _onboard_mission_allowed; + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD, + } _current_mission_type; + + orb_advert_t _mission_result_pub; /**< publish mission result topic */ + mission_result_s _mission_result; /**< mission result for commander/mavlink */ +}; + +#endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 55f8a4caa..5f113f686 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -39,7 +39,9 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ - navigator_mission.cpp \ + mission.cpp \ + rtl.cpp \ + rtl_params.c \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 494266dd3..34a28aec3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Jean Cyr - * @author Julian Oes - * @author Anton Babushkin + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,19 +31,20 @@ * ****************************************************************************/ /** - * @file navigator_main.c + * @file navigator_main.cpp * Implementation of the main navigation state machine. * - * Handles missions, geo fencing and failsafe navigation behavior. - * Published the mission item triplet for the position controller. + * Handles mission items, geo fencing and failsafe navigation behavior. + * Published the position setpoint triplet for the position controller. * * @author Lorenz Meier * @author Jean Cyr - * @author Julian Oes + * @author Julian Oes * @author Anton Babushkin */ #include + #include #include #include @@ -58,9 +55,13 @@ #include #include #include +#include +#include + #include #include #include + #include #include #include @@ -71,20 +72,19 @@ #include #include #include + #include #include #include #include #include #include -#include #include +#include #include -#include -#include -#include "navigator_state.h" -#include "navigator_mission.h" +#include "mission.h" +#include "rtl.h" #include "mission_feasibility_checker.h" #include "geofence.h" @@ -130,12 +130,12 @@ public: /** * Add point to geofence */ - void add_fence_point(int argc, char *argv[]); + void add_fence_point(int argc, char *argv[]); /** * Load fence from file */ - void load_fence_from_file(const char *filename); + void load_fence_from_file(const char *filename); private: @@ -154,15 +154,15 @@ private: int _control_mode_sub; /**< vehicle control mode subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ - orb_advert_t _mission_result_pub; /**< publish mission result topic */ - struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct home_position_s _home_pos; /**< home position for RTL */ - struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ - struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ - struct mission_item_s _mission_item; /**< current mission item */ + vehicle_status_s _vstatus; /**< vehicle status */ + vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + vehicle_global_position_s _global_pos; /**< global vehicle position */ + home_position_s _home_pos; /**< home position for RTL */ + position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + mission_item_s _mission_item; /**< current mission item */ + + bool _mission_item_valid; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -174,56 +174,41 @@ private: struct navigation_capabilities_s _nav_caps; - class Mission _mission; + Mission _mission; + + RTL _rtl; - bool _mission_item_valid; /**< current mission item valid */ - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ - bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ - bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ - MissionFeasibilityChecker missionFeasiblityChecker; - uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ - - bool _pos_sp_triplet_updated; - - const char *nav_states_str[NAV_STATE_MAX]; + bool _update_triplet; struct { - float min_altitude; float acceptance_radius; float loiter_radius; int onboard_mission_enabled; float takeoff_alt; - float land_alt; - float rtl_alt; - float rtl_land_delay; } _parameters; /**< local copies of parameters */ struct { - param_t min_altitude; param_t acceptance_radius; param_t loiter_radius; param_t onboard_mission_enabled; param_t takeoff_alt; - param_t land_alt; - param_t rtl_alt; - param_t rtl_land_delay; } _parameter_handles; /**< handles for parameters */ enum Event { EVENT_NONE_REQUESTED, - EVENT_READY_REQUESTED, EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, - EVENT_LAND_REQUESTED, + EVENT_TAKEN_OFF, + EVENT_LANDED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, + EVENT_MISSION_ITEM_REACHED, MAX_EVENT }; @@ -232,15 +217,6 @@ private: */ static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND - }; - - enum RTLState _rtl_state; - /** * Update our local parameter cache. */ @@ -264,7 +240,7 @@ private: /** * Retrieve offboard mission. */ - void offboard_mission_update(bool isrotaryWing); + void offboard_mission_update(); /** * Retrieve onboard mission. @@ -296,19 +272,20 @@ private: /** * Functions that are triggered when a new state is entered. */ - void start_none(); - void start_ready(); - void start_loiter(); - void start_mission(); - void start_rtl(); - void start_land(); - void start_land_home(); + bool start_none_on_ground(); + bool start_none_in_air(); + bool start_auto_on_ground(); + bool start_loiter(); + bool start_mission(); + bool advance_mission(); + bool start_rtl(); + bool advance_rtl(); + bool start_land(); /** * Fork for state transitions */ - void request_loiter_or_ready(); - void request_mission_if_available(); + // void request_loiter_or_ready(); /** * Guards offboard mission @@ -333,12 +310,12 @@ private: /** * Perform actions when current mission item reached. */ - void on_mission_item_reached(); + // void on_mission_item_reached(); /** * Move to next waypoint */ - void set_mission_item(); + bool set_mission_items(); /** * Switch to next RTL state @@ -348,7 +325,7 @@ private: /** * Set position setpoint for mission item */ - void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item); + void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); /** * Helper function to get a takeoff item @@ -377,12 +354,9 @@ Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), - _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), - -/* subscriptions */ _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), @@ -391,53 +365,29 @@ Navigator::Navigator() : _onboard_mission_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), - -/* publications */ _pos_sp_triplet_pub(-1), - -/* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), - _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), _mission(), + _pos_sp_triplet({}), + _mission_item({}), _mission_item_valid(false), - _global_pos_valid(false), - _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _need_takeoff(true), - _do_takeoff(false), - _set_nav_state_timestamp(0), - _pos_sp_triplet_updated(false), -/* states */ - _rtl_state(RTL_STATE_NONE) + _update_triplet(false) { - _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); - _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); - _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); - - memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); - memset(&_mission_item, 0, sizeof(struct mission_item_s)); - - memset(&nav_states_str, 0, sizeof(nav_states_str)); - nav_states_str[0] = "NONE"; - nav_states_str[1] = "READY"; - nav_states_str[2] = "LOITER"; - nav_states_str[3] = "MISSION"; - nav_states_str[4] = "RTL"; - nav_states_str[5] = "LAND"; /* Initialize state machine */ - myState = NAV_STATE_NONE; - start_none(); + myState = NAV_STATE_NONE_ON_GROUND; + + start_none_on_ground(); } Navigator::~Navigator() @@ -472,16 +422,12 @@ Navigator::parameters_update() struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); - param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); - param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); - param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); - param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay)); + param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); + _mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); _geofence.updateParams(); } @@ -496,6 +442,8 @@ void Navigator::home_position_update() { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + + _rtl.set_home_position(&_home_pos); } void @@ -506,7 +454,7 @@ Navigator::navigation_capabilities_update() void -Navigator::offboard_mission_update(bool isrotaryWing) +Navigator::offboard_mission_update() { struct mission_s offboard_mission; @@ -523,19 +471,17 @@ Navigator::offboard_mission_update(bool isrotaryWing) dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); + missionFeasiblityChecker.checkMissionFeasible(_vstatus.is_rotary_wing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_offboard_mission_count(offboard_mission.count); - _mission.set_current_offboard_mission_index(offboard_mission.current_index); + _mission.command_current_offboard_mission_index(offboard_mission.current_index); } else { _mission.set_offboard_mission_count(0); - _mission.set_current_offboard_mission_index(0); + _mission.command_current_offboard_mission_index(0); } - - _mission.publish_mission_result(); } void @@ -546,11 +492,11 @@ Navigator::onboard_mission_update() if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_onboard_mission_count(onboard_mission.count); - _mission.set_current_onboard_mission_index(onboard_mission.current_index); + _mission.command_current_onboard_mission_index(onboard_mission.current_index); } else { _mission.set_onboard_mission_count(0); - _mission.set_current_onboard_mission_index(0); + _mission.command_current_onboard_mission_index(0); } } @@ -626,13 +572,13 @@ Navigator::task_main() global_position_update(); home_position_update(); navigation_capabilities_update(); - offboard_mission_update(_vstatus.is_rotary_wing); + offboard_mission_update(); onboard_mission_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = NAV_STATE_NONE; + unsigned prevState = NAV_STATE_NONE_ON_GROUND; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -690,55 +636,45 @@ Navigator::task_main() if (fds[6].revents & POLLIN) { vehicle_status_update(); - /* evaluate state requested by commander */ - if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - request_loiter_or_ready(); - break; - - case NAV_STATE_MISSION: - request_mission_if_available(); - break; - - case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } - - break; - - case NAV_STATE_LAND: - dispatch(EVENT_LAND_REQUESTED); - - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - request_mission_if_available(); - } - } - - } else { - /* navigator shouldn't act */ + /* commander requested new navigation mode, forward it to state machine */ + switch (_vstatus.set_nav_state) { + case NAVIGATION_STATE_NONE: dispatch(EVENT_NONE_REQUESTED); + break; + + case NAVIGATION_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAVIGATION_STATE_MISSION: + dispatch(EVENT_MISSION_REQUESTED); + break; + + case NAVIGATION_STATE_RTL: + /* TODO: what is this for? */ + // if (!(_rtl_state == RTL_STATE_DESCEND && + // (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + // _vstatus.condition_home_position_valid) { + dispatch(EVENT_RTL_REQUESTED); + // } + break; + + case NAVIGATION_STATE_LAND: + /* TODO: add this */ + // dispatch(EVENT_LAND_REQUESTED); + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; } - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + /* commander sets in-air/on-ground flag as well */ + if (_vstatus.condition_landed) { + dispatch(EVENT_LANDED); + } else { + dispatch(EVENT_TAKEN_OFF); + } } /* parameters updated */ @@ -754,74 +690,49 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { - offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + offboard_mission_update(); + /* TODO: check if mission really changed */ dispatch(EVENT_MISSION_CHANGED); } /* onboard mission updated */ if (fds[5].revents & POLLIN) { onboard_mission_update(); - // XXX check if mission really changed + /* TODO: check if mission really changed */ dispatch(EVENT_MISSION_CHANGED); } /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); - // XXX check if home position really changed + /* TODO check if home position really changed */ dispatch(EVENT_HOME_POSITION_CHANGED); } /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); - - /* publish position setpoint triplet on each position update if navigator active */ - if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - _pos_sp_triplet_updated = true; - - if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { - /* got global position when landing, update setpoint */ - start_land(); - } - - _global_pos_valid = _global_pos.global_valid; - - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { - if (check_mission_item_reached()) { - on_mission_item_reached(); - } - } + if (check_mission_item_reached()) { + dispatch(EVENT_MISSION_ITEM_REACHED); } /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { - //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } - } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } - /* publish position setpoint triplet if updated */ - if (_pos_sp_triplet_updated) { - _pos_sp_triplet_updated = false; + if (_update_triplet ) { publish_position_setpoint_triplet(); - } - - /* notify user about state changes */ - if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]); - prevState = myState; + _update_triplet = false; } perf_end(_loop_perf); @@ -879,8 +790,12 @@ Navigator::status() } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); + case NAV_STATE_NONE_ON_GROUND: + warnx("State: None on ground"); + break; + + case NAV_STATE_NONE_IN_AIR: + warnx("State: None in air"); break; case NAV_STATE_LOITER: @@ -895,6 +810,10 @@ Navigator::status() warnx("State: RTL"); break; + case NAV_STATE_LAND: + warnx("State: LAND"); + break; + default: warnx("State: Unknown"); break; @@ -903,92 +822,119 @@ Navigator::status() StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { - /* NAV_STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* NAV_STATE_NONE_ON_GROUND */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + }, + { + /* NAV_STATE_NONE_IN_AIR */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, + /* EVENT_LANDED */ {ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, }, { - /* NAV_STATE_READY */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, + /* NAV_STATE_AUTO_ON_GROUND */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, }, { /* NAV_STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LOITER}, }, { /* NAV_STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_mission), NAV_STATE_MISSION}, }, { /* NAV_STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_rtl), NAV_STATE_RTL}, }, { /* NAV_STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_LANDED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LAND}, }, }; -void -Navigator::start_none() +bool +Navigator::start_none_on_ground() { reset_reached(); _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; - _mission_item_valid = false; - _reset_loiter_pos = true; - _do_takeoff = false; - _rtl_state = RTL_STATE_NONE; + _update_triplet = true; + return true; +} + +bool +Navigator::start_none_in_air() +{ + reset_reached(); + + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; - _pos_sp_triplet_updated = true; + _update_triplet = true; + return true; } -void -Navigator::start_ready() +bool +Navigator::start_auto_on_ground() { reset_reached(); @@ -998,46 +944,26 @@ Navigator::start_ready() _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; - _mission_item_valid = false; - - _reset_loiter_pos = true; - _do_takeoff = false; - if (_rtl_state != RTL_STATE_DESCEND) { - /* reset RTL state if landed not at home */ - _rtl_state = RTL_STATE_NONE; - } + // if (_rtl_state != RTL_STATE_DESCEND) { + // reset RTL state if landed not at home + // _rtl_state = RTL_STATE_NONE; + // } - _pos_sp_triplet_updated = true; + _update_triplet = true; + return true; } -void +bool Navigator::start_loiter() { - reset_reached(); - - _do_takeoff = false; - - /* set loiter position if needed */ - if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { - _reset_loiter_pos = false; + /* if no existing item available, use current position */ + if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) { _pos_sp_triplet.current.lat = _global_pos.lat; _pos_sp_triplet.current.lon = _global_pos.lon; _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - - float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; - - /* use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { - _pos_sp_triplet.current.alt = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); - - } else { - _pos_sp_triplet.current.alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); - } - + _pos_sp_triplet.current.alt = _global_pos.alt; } _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; @@ -1045,167 +971,146 @@ Navigator::start_loiter() _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; - _mission_item_valid = false; - _pos_sp_triplet_updated = true; + mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); + + _update_triplet = true; + return true; } -void +bool Navigator::start_mission() { - _need_takeoff = true; + /* start fresh */ + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; - set_mission_item(); + return set_mission_items(); } -void -Navigator::set_mission_item() +bool +Navigator::advance_mission() { - reset_reached(); + /* tell mission to move by one */ + _mission.move_to_next(); - /* copy current mission to previous item */ - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + /* now try to set the new mission items, if it fails, it will dispatch loiter */ + return set_mission_items(); +} - _reset_loiter_pos = true; - _do_takeoff = false; +bool +Navigator::set_mission_items() +{ + if (_pos_sp_triplet.current.valid) { + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + _pos_sp_triplet.previous.valid = true; + } - int ret; bool onboard; - unsigned index; + int index; - ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); + /* if we fail to set the current mission, continue to loiter */ + if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) { - if (ret == OK) { - _mission.report_current_offboard_mission_item(); + dispatch(EVENT_LOITER_REQUESTED); + return false; + } - _mission_item_valid = true; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { - /* don't reset RTL state on RTL or LOITER items */ - _rtl_state = RTL_STATE_NONE; - } + /* if we got an RTL mission item, switch to RTL mode and give up */ + if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + dispatch(EVENT_RTL_REQUESTED); + return false; + } - if (_vstatus.is_rotary_wing) { - if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { - /* do special TAKEOFF handling for VTOL */ - _need_takeoff = false; - - /* calculate desired takeoff altitude AMSL */ - float takeoff_alt_amsl = _pos_sp_triplet.current.alt; - - if (_vstatus.condition_landed) { - /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ - takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt); - } + _mission_item_valid = true; - /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) { - /* force TAKEOFF if landed or waypoint altitude is more than current */ - _do_takeoff = true; + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); + _pos_sp_triplet.current.valid = true; + - /* move current position setpoint to next */ - memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + mission_item_s next_mission_item; - /* set current setpoint to takeoff */ + bool last_wp = false; + /* now try to set the next mission item as well, if there is no more next + * this means we're heading to the last waypoint */ + if (_mission.get_next_mission_item(&next_mission_item)) { + /* convert the next mission item and set it valid */ + mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next); + _pos_sp_triplet.next.valid = true; + } else { + last_wp = true; + } - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.alt = takeoff_alt_amsl; - _pos_sp_triplet.current.yaw = NAN; - _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; - } + /* notify user about what happened */ + mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d", + (last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index); - } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { - /* will need takeoff after landing */ - _need_takeoff = true; - } - } + _update_triplet = true; - if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); + reset_reached(); - } else { - if (onboard) { - mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index); + return true; +} - } else { - mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index); - } - } +bool +Navigator::start_rtl() +{ + /* TODO check if RTL was successfull, if not we have a problem and need to dispatch something */ + if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - } else { - /* since a mission is not advanced without WPs available, this is not supposed to happen */ - _mission_item_valid = false; - _pos_sp_triplet.current.valid = false; - warnx("ERROR: current WP can't be set"); - } + _mission_item_valid = true; - if (!_do_takeoff) { - mission_item_s item_next; - ret = _mission.get_next_mission_item(&item_next); + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); + _pos_sp_triplet.current.valid = true; - if (ret == OK) { - position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); + reset_reached(); - } else { - /* this will fail for the last WP */ - _pos_sp_triplet.next.valid = false; - } + _update_triplet = true; + return true; } + dispatch(EVENT_LOITER_REQUESTED); - _pos_sp_triplet_updated = true; + return false; } -void -Navigator::start_rtl() +bool +Navigator::advance_rtl() { - _do_takeoff = false; + /* tell mission to move by one */ + _rtl.move_to_next(); - /* decide if we need climb */ - if (_rtl_state == RTL_STATE_NONE) { - if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { - _rtl_state = RTL_STATE_CLIMB; + /* now try to set the new mission items, if it fails, it will dispatch loiter */ + if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - } else { - _rtl_state = RTL_STATE_RETURN; - } - } + _mission_item_valid = true; - /* if switching directly to return state, reset altitude setpoint */ - if (_rtl_state == RTL_STATE_RETURN) { - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); + _pos_sp_triplet.current.valid = true; + + reset_reached(); + + _update_triplet = true; + return true; } - _reset_loiter_pos = true; - set_rtl_item(); + dispatch(EVENT_LOITER_REQUESTED); + return false; } -void +bool Navigator::start_land() { + /* TODO: verify/test */ + reset_reached(); /* this state can be requested by commander even if no global position available, * in his case controller must perform landing without position control */ - _do_takeoff = false; - _reset_loiter_pos = true; memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; - _mission_item.lat = _global_pos.lat; _mission_item.lon = _global_pos.lon; _mission_item.altitude_is_relative = false; @@ -1220,185 +1125,22 @@ Navigator::start_land() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; -} - -void -Navigator::start_land_home() -{ - reset_reached(); - - /* land to home position, should be called when hovering above home, from RTL state */ - _do_takeoff = false; - _reset_loiter_pos = true; - - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); _pos_sp_triplet.next.valid = false; -} - -void -Navigator::set_rtl_item() -{ - reset_reached(); - - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - float climb_alt = _home_pos.alt + _parameters.rtl_alt; - - if (_vstatus.condition_landed) { - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - } - - _mission_item_valid = true; - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); - break; - } - - case RTL_STATE_RETURN: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - // don't change altitude - if (_pos_sp_triplet.previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); - - } else { - /* else use current position */ - _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); - } - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); - break; - } - - case RTL_STATE_DESCEND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt + _parameters.land_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); - break; - } - - default: { - mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } - } - - _pos_sp_triplet_updated = true; -} - -void -Navigator::request_loiter_or_ready() -{ - /* XXX workaround: no landing detector for fixedwing yet */ - if (_vstatus.condition_landed && _vstatus.is_rotary_wing) { - dispatch(EVENT_READY_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } -} - -void -Navigator::request_mission_if_available() -{ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - request_loiter_or_ready(); - } + _update_triplet = true; + return true; } void -Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) +Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { sp->valid = true; if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* set home position for RTL item */ - sp->lat = _home_pos.lat; - sp->lon = _home_pos.lon; - sp->alt = _home_pos.alt + _parameters.rtl_alt; if (_pos_sp_triplet.previous.valid) { /* if previous setpoint is valid then use it to calculate heading to home */ @@ -1408,9 +1150,6 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ /* else use current position */ sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon); } - sp->loiter_radius = _parameters.loiter_radius; - sp->loiter_direction = 1; - sp->pitch_min = 0.0f; } else { sp->lat = item->lat; @@ -1450,14 +1189,14 @@ Navigator::check_mission_item_reached() return _vstatus.condition_landed; } - /* XXX TODO count turns */ - if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item.loiter_radius > 0.01f) { + /* TODO count turns */ + // if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + // _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + // _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + // _mission_item.loiter_radius > 0.01f) { - return false; - } + // return false; + // } uint64_t now = hrt_absolute_time(); @@ -1475,22 +1214,19 @@ Navigator::check_mission_item_reached() float dist_xy = -1.0f; float dist_z = -1.0f; - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; + float altitude_amsl = _mission_item.altitude_is_relative + ? _mission_item.altitude + _home_pos.alt : _mission_item.altitude; - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, + _global_pos.lat, _global_pos.lon, _global_pos.alt, &dist_xy, &dist_z); - if (_do_takeoff) { - if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { - /* require only altitude for takeoff */ + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + + /* require only altitude for takeoff */ + if (_global_pos.alt > altitude_amsl - acceptance_radius) { _waypoint_position_reached = true; } - } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1499,7 +1235,10 @@ Navigator::check_mission_item_reached() } if (_waypoint_position_reached && !_waypoint_yaw_reached) { - if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { + + /* TODO: removed takeoff, why? */ + if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) { + /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); @@ -1514,24 +1253,23 @@ Navigator::check_mission_item_reached() /* check if the current waypoint was reached */ if (_waypoint_position_reached && _waypoint_yaw_reached) { + if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", + (double)_mission_item.time_inside); } } /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - reset_reached(); return true; } } - return false; - } void @@ -1543,62 +1281,6 @@ Navigator::reset_reached() } -void -Navigator::on_mission_item_reached() -{ - if (myState == NAV_STATE_MISSION) { - - _mission.report_mission_item_reached(); - - if (_do_takeoff) { - /* takeoff completed */ - _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "#audio: takeoff completed"); - - } else { - /* advance by one mission item */ - _mission.move_to_next(); - } - - if (_mission.current_mission_available()) { - set_mission_item(); - - } else { - /* if no more mission items available then finish mission */ - /* loiter at last waypoint */ - _reset_loiter_pos = false; - mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - request_loiter_or_ready(); - } - - } else if (myState == NAV_STATE_RTL) { - /* RTL completed */ - if (_rtl_state == RTL_STATE_DESCEND) { - /* hovering above home position, land if needed or loiter */ - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); - - if (_mission_item.autocontinue) { - dispatch(EVENT_LAND_REQUESTED); - - } else { - _reset_loiter_pos = false; - dispatch(EVENT_LOITER_REQUESTED); - } - - } else { - /* next RTL step */ - _rtl_state = (RTLState)(_rtl_state + 1); - set_rtl_item(); - } - - } else if (myState == NAV_STATE_LAND) { - /* landing completed */ - mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); - dispatch(EVENT_READY_REQUESTED); - } - _mission.publish_mission_result(); -} - void Navigator::publish_position_setpoint_triplet() { @@ -1607,11 +1289,9 @@ Navigator::publish_position_setpoint_triplet() /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { - /* publish the position setpoint triplet */ orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { - /* advertise and publish */ _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp deleted file mode 100644 index ac7e604ef..000000000 --- a/src/modules/navigator/navigator_mission.cpp +++ /dev/null @@ -1,332 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file navigator_mission.cpp - * Helper class to access missions - */ - -#include -#include -#include -#include -#include -#include -#include "navigator_mission.h" - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - - -Mission::Mission() : - - _offboard_dataman_id(-1), - _current_offboard_mission_index(0), - _current_onboard_mission_index(0), - _offboard_mission_item_count(0), - _onboard_mission_item_count(0), - _onboard_mission_allowed(false), - _current_mission_type(MISSION_TYPE_NONE), - _mission_result_pub(-1) -{ - memset(&_mission_result, 0, sizeof(struct mission_result_s)); -} - -Mission::~Mission() -{ - -} - -void -Mission::set_offboard_dataman_id(int new_id) -{ - _offboard_dataman_id = new_id; -} - -void -Mission::set_current_offboard_mission_index(int new_index) -{ - if (new_index != -1) { - warnx("specifically set to %d", new_index); - _current_offboard_mission_index = (unsigned)new_index; - } else { - - /* if less WPs available, reset to first WP */ - if (_current_offboard_mission_index >= _offboard_mission_item_count) { - _current_offboard_mission_index = 0; - } - } - report_current_offboard_mission_item(); -} - -void -Mission::set_current_onboard_mission_index(int new_index) -{ - if (new_index != -1) { - _current_onboard_mission_index = (unsigned)new_index; - } else { - - /* if less WPs available, reset to first WP */ - if (_current_onboard_mission_index >= _onboard_mission_item_count) { - _current_onboard_mission_index = 0; - } - } - // TODO: implement this for onboard missions as well - // report_current_mission_item(); -} - -void -Mission::set_offboard_mission_count(unsigned new_count) -{ - _offboard_mission_item_count = new_count; -} - -void -Mission::set_onboard_mission_count(unsigned new_count) -{ - _onboard_mission_item_count = new_count; -} - -void -Mission::set_onboard_mission_allowed(bool allowed) -{ - _onboard_mission_allowed = allowed; -} - -bool -Mission::current_mission_available() -{ - return (current_onboard_mission_available() || current_offboard_mission_available()); - -} - -bool -Mission::next_mission_available() -{ - return (next_onboard_mission_available() || next_offboard_mission_available()); -} - -int -Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index) -{ - int ret = ERROR; - - /* try onboard mission first */ - if (current_onboard_mission_available()) { - - ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &_current_onboard_mission_index, new_mission_item); - if (ret == OK) { - _current_mission_type = MISSION_TYPE_ONBOARD; - *onboard = true; - *index = _current_onboard_mission_index; - } - - /* otherwise fallback to offboard */ - } else if (current_offboard_mission_available()) { - - dm_item_t dm_current; - - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - ret = get_mission_item(dm_current, &_current_offboard_mission_index, new_mission_item); - if (ret == OK) { - _current_mission_type = MISSION_TYPE_OFFBOARD; - *onboard = false; - *index = _current_offboard_mission_index; - } - - } else { - /* happens when no more mission items can be added as a next item */ - _current_mission_type = MISSION_TYPE_NONE; - ret == ERROR; - } - - return ret; -} - -int -Mission::get_next_mission_item(struct mission_item_s *new_mission_item) -{ - int ret = ERROR; - - /* try onboard mission first */ - if (next_onboard_mission_available()) { - - unsigned next_onboard_mission_index = _current_onboard_mission_index + 1; - ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &next_onboard_mission_index, new_mission_item); - - /* otherwise fallback to offboard */ - } else if (next_offboard_mission_available()) { - - dm_item_t dm_current; - - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - unsigned next_offboard_mission_index = _current_offboard_mission_index + 1; - ret = get_mission_item(dm_current, &next_offboard_mission_index, new_mission_item); - - } else { - /* happens when no more mission items can be added as a next item */ - ret = ERROR; - } - return ret; -} - - -bool -Mission::current_onboard_mission_available() -{ - return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed; -} - -bool -Mission::current_offboard_mission_available() -{ - return _offboard_mission_item_count > _current_offboard_mission_index; -} - -bool -Mission::next_onboard_mission_available() -{ - unsigned next = 0; - - if (_current_mission_type != MISSION_TYPE_ONBOARD) { - next = 1; - } - - return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed; -} - -bool -Mission::next_offboard_mission_available() -{ - unsigned next = 0; - - if (_current_mission_type != MISSION_TYPE_OFFBOARD) { - next = 1; - } - - return _offboard_mission_item_count > (_current_offboard_mission_index + next); -} - -void -Mission::move_to_next() -{ - switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - - case MISSION_TYPE_NONE: - default: - break; - } -} - -void -Mission::report_mission_item_reached() -{ - if (_current_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; - } -} - -void -Mission::report_current_offboard_mission_item() -{ - _mission_result.index_current_mission = _current_offboard_mission_index; -} - -void -Mission::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); - } - /* reset reached bool */ - _mission_result.mission_reached = false; -} - -int -Mission::get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item) -{ - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ - for (int i=0; i<10; i++) { - - const ssize_t len = sizeof(struct mission_item_s); - - /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } - - /* check for DO_JUMP item */ - if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; - } else { - /* if it's not a DO_JUMP, then we were successful */ - return OK; - } - } - - /* we have given up, we don't want to cycle forever */ - warnx("ERROR: cycling through mission items without success"); - return ERROR; -} diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h deleted file mode 100644 index fef145410..000000000 --- a/src/modules/navigator/navigator_mission.h +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file navigator_mission.h - * Helper class to access missions - */ - -#ifndef NAVIGATOR_MISSION_H -#define NAVIGATOR_MISSION_H - -#include -#include - - -class __EXPORT Mission -{ -public: - /** - * Constructor - */ - Mission(); - - /** - * Destructor, also kills the sensors task. - */ - ~Mission(); - - void set_offboard_dataman_id(int new_id); - void set_current_offboard_mission_index(int new_index); - void set_current_onboard_mission_index(int new_index); - void set_offboard_mission_count(unsigned new_count); - void set_onboard_mission_count(unsigned new_count); - - void set_onboard_mission_allowed(bool allowed); - - bool current_mission_available(); - bool next_mission_available(); - - int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index); - int get_next_mission_item(struct mission_item_s *mission_item); - - void move_to_next(); - - void report_mission_item_reached(); - void report_current_offboard_mission_item(); - void publish_mission_result(); - -private: - int get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item); - - bool current_onboard_mission_available(); - bool current_offboard_mission_available(); - bool next_onboard_mission_available(); - bool next_offboard_mission_available(); - - int _offboard_dataman_id; - unsigned _current_offboard_mission_index; - unsigned _current_onboard_mission_index; - unsigned _offboard_mission_item_count; /** number of offboard mission items available */ - unsigned _onboard_mission_item_count; /** number of onboard mission items available */ - - bool _onboard_mission_allowed; - - enum { - MISSION_TYPE_NONE, - MISSION_TYPE_ONBOARD, - MISSION_TYPE_OFFBOARD, - } _current_mission_type; - - int _mission_result_pub; - - struct mission_result_s _mission_result; -}; - -#endif diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9ef359c6d..49969a382 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -52,16 +52,6 @@ * Navigator parameters, accessible via MAVLink */ -/** - * Minimum altitude (fixed wing only) - * - * Minimum altitude above home for LOITER. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); - /** * Waypoint acceptance radius * @@ -101,37 +91,6 @@ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); */ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); -/** - * Landing altitude - * - * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); - -/** - * Return-To-Launch altitude - * - * Minimum altitude above home position for going home in RTL mode. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); - -/** - * Return-To-Launch delay - * - * Delay after descend before landing in RTL mode. - * If set to -1 the system will not land but loiter at NAV_LAND_ALT. - * - * @unit seconds - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); - /** * Enable parachute deployment * diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h deleted file mode 100644 index 6a1475c9b..000000000 --- a/src/modules/navigator/navigator_state.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * navigator_state.h - * - * Created on: 27.01.2014 - * Author: ton - */ - -#ifndef NAVIGATOR_STATE_H_ -#define NAVIGATOR_STATE_H_ - -typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX -} nav_state_t; - -#endif /* NAVIGATOR_STATE_H_ */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp new file mode 100644 index 000000000..6e621e39d --- /dev/null +++ b/src/modules/navigator/rtl.cpp @@ -0,0 +1,253 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_rtl.cpp + * Helper class to access RTL + * @author Julian Oes + * @author Anton Babushkin + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include "rtl.h" + + +RTL::RTL() : + SuperBlock(NULL, "RTL"), + _mavlink_fd(-1), + _rtl_state(RTL_STATE_NONE), + _home_position({}), + _param_return_alt(this, "RETURN_ALT"), + _param_descend_alt(this, "DESCEND_ALT"), + _param_land_delay(this, "LAND_DELAY"), + _loiter_radius(50), + _acceptance_radius(50) +{ + /* load initial params */ + updateParams(); +} + +RTL::~RTL() +{ +} + +void +RTL::set_home_position(const home_position_s *new_home_position) +{ + memcpy(&_home_position, new_home_position, sizeof(home_position_s)); +} + +bool +RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item) +{ + /* Open mavlink fd */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + + /* decide if we need climb */ + if (_rtl_state == RTL_STATE_NONE) { + if (global_position->alt < _home_position.alt + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + } else { + _rtl_state = RTL_STATE_RETURN; + } + } + + /* if switching directly to return state, set altitude setpoint to current altitude */ + if (_rtl_state == RTL_STATE_RETURN) { + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = global_position->alt; + } + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + + float climb_alt = _home_position.alt + _param_return_alt.get(); + + /* TODO understand and fix this */ + // if (_vstatus.condition_landed) { + // climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + // } + + new_mission_item->lat = global_position->lat; + new_mission_item->lon = global_position->lon; + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = climb_alt; + new_mission_item->yaw = NAN; + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_TAKEOFF; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = 0.0f; + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = true; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %d meters above home", + (int)(climb_alt - _home_position.alt)); + break; + } + case RTL_STATE_RETURN: { + + new_mission_item->lat = _home_position.lat; + new_mission_item->lon = _home_position.lon; + + /* TODO: add this again */ + // don't change altitude + // if (_pos_sp_triplet.previous.valid) { + // /* if previous setpoint is valid then use it to calculate heading to home */ + // new_mission_item->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, new_mission_item->lat, new_mission_item->lon); + + // } else { + // /* else use current position */ + // new_mission_item->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, new_mission_item->lat, new_mission_item->lon); + // } + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = 0.0f; + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = true; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %d meters above home", + (int)(new_mission_item->altitude - _home_position.alt)); + break; + } + + case RTL_STATE_DESCEND: { + + new_mission_item->lat = _home_position.lat; + new_mission_item->lon = _home_position.lon; + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = _home_position.alt + _param_descend_alt.get(); + new_mission_item->yaw = NAN; + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = _param_land_delay.get() > -0.001f; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %d meters above home", + (int)(new_mission_item->altitude - _home_position.alt)); + break; + } + + case RTL_STATE_LAND: { + + new_mission_item->lat = _home_position.lat; + new_mission_item->lon = _home_position.lon; + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = _home_position.alt; + new_mission_item->yaw = NAN; + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_LAND; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = 0.0f; + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = true; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: land at home"); + break; + } + + case RTL_STATE_FINISHED: { + /* nothing to do, report fail */ + return false; + } + + default: + return false; + } + + return true; +} + +bool +RTL::get_next_rtl_item(mission_item_s *new_mission_item) +{ + /* TODO implement */ + return false; +} + +void +RTL::move_to_next() +{ + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + _rtl_state = RTL_STATE_DESCEND; + break; + + case RTL_STATE_DESCEND: + /* only go to land if autoland is enabled */ + if (_param_land_delay.get() < 0) { + _rtl_state = RTL_STATE_FINISHED; + } else { + _rtl_state = RTL_STATE_LAND; + } + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_FINISHED; + break; + + case RTL_STATE_FINISHED: + break; + + default: + break; + } +} \ No newline at end of file diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h new file mode 100644 index 000000000..c761837fc --- /dev/null +++ b/src/modules/navigator/rtl.h @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.h + * Helper class to access missions + * @author Julian Oes + * @author Anton Babushkin + */ + +#ifndef NAVIGATOR_RTL_H +#define NAVIGATOR_RTL_H + +#include +#include + +#include +#include +#include + +class RTL : public control::SuperBlock +{ +public: + /** + * Constructor + */ + RTL(); + + /** + * Destructor + */ + ~RTL(); + + void set_home_position(const home_position_s *home_position); + + bool get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item); + bool get_next_rtl_item(mission_item_s *mission_item); + + void move_to_next(); + +private: + int _mavlink_fd; + + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LAND, + RTL_STATE_FINISHED, + } _rtl_state; + + home_position_s _home_position; + float _loiter_radius; + float _acceptance_radius; + + + control::BlockParamFloat _param_return_alt; + control::BlockParamFloat _param_descend_alt; + control::BlockParamFloat _param_land_delay; +}; + +#endif diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c new file mode 100644 index 000000000..7a8b1d745 --- /dev/null +++ b/src/modules/navigator/rtl_params.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rtl_params.c + * + * Parameters for RTL + * + * @author Julian Oes + */ + +#include + +#include + +/* + * RTL parameters, accessible via MAVLink + */ + +/** + * RTL altitude + * + * Altitude to fly back in RTL in meters + * + * @unit meters + * @min 0 + * @max 1 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); + + +/** + * RTL loiter altitude + * + * Stay at this altitude above home position after RTL descending. + * Land (i.e. slowly descend) from this altitude if autolanding allowed. + * + * @unit meters + * @min 0 + * @max 100 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); + +/** + * RTL delay + * + * Delay after descend before landing in RTL mode. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @unit seconds + * @min -1 + * @max + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index f2709d29f..38378651b 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,7 @@ * @file state_table.h * * Finite-State-Machine helper class for state table + * @author: Julian Oes */ #ifndef __SYSTEMLIB_STATE_TABLE_H @@ -43,7 +44,7 @@ class StateTable { public: - typedef void (StateTable::*Action)(); + typedef bool (StateTable::*Action)(); struct Tran { Action action; unsigned nextState; @@ -53,17 +54,23 @@ public: : myTable(table), myNsignals(nSignals), myNstates(nStates) {} #define NO_ACTION &StateTable::doNothing - #define ACTION(_target) static_cast(_target) + #define ACTION(_target) StateTable::Action(_target) virtual ~StateTable() {} void dispatch(unsigned const sig) { - register Tran const *t = myTable + myState*myNsignals + sig; - (this->*(t->action))(); - - myState = t->nextState; + /* get transition using state table */ + Tran const *t = myTable + myState*myNsignals + sig; + /* first up change state, this allows to do further dispatchs in the state functions */ + + /* now execute state function, if it runs with success, accept new state */ + if ((this->*(t->action))()) { + myState = t->nextState; + } + } + bool doNothing() { + return true; } - void doNothing() {} protected: unsigned myState; private: diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index dfc461ae4..4532b329d 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -93,6 +93,8 @@ struct mission_item_s { bool autocontinue; /**< true if next waypoint should follow after this one */ enum ORIGIN origin; /**< where the waypoint has been generated */ int do_jump_mission_index; /**< the mission index that we want to jump to */ + int do_jump_repeat_count; /**< how many times the jump should be repeated */ + int do_jump_current_count; /**< how many times the jump has already been repeated */ }; struct mission_s diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 34aaa30dd..85e8ef8a5 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -45,7 +45,6 @@ #include #include #include "../uORB.h" -#include /** * @addtogroup topics @@ -74,6 +73,17 @@ struct position_setpoint_s float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ }; +typedef enum { + NAV_STATE_NONE_ON_GROUND = 0, + NAV_STATE_NONE_IN_AIR, + NAV_STATE_AUTO_ON_GROUND, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_RTL, + NAV_STATE_LAND, + NAV_STATE_MAX +} nav_state_t; + /** * Global position setpoint triplet in WGS84 coordinates. * diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index adcd028f0..cfab695f8 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -36,7 +36,7 @@ * Definition of the global fused WGS84 position uORB topic. * * @author Thomas Gubler - * @author Julian Oes + * @author Julian Oes * @author Lorenz Meier */ @@ -66,16 +66,16 @@ struct vehicle_global_position_s { bool global_valid; /**< true if position satisfies validity criteria of estimator */ bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude AMSL in meters */ - float vel_n; /**< Ground north velocity, m/s */ - float vel_e; /**< Ground east velocity, m/s */ - float vel_d; /**< Ground downside velocity, m/s */ + float vel_n; /**< Ground north velocity, m/s */ + float vel_e; /**< Ground east velocity, m/s */ + float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ + float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 48af4c9e2..c1bd828d8 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,8 +54,6 @@ #include #include "../uORB.h" -#include - /** * @addtogroup topics @{ */ @@ -93,6 +91,14 @@ typedef enum { FAILSAFE_STATE_MAX } failsafe_state_t; +typedef enum { + NAVIGATION_STATE_NONE = 0, + NAVIGATION_STATE_MISSION, + NAVIGATION_STATE_LOITER, + NAVIGATION_STATE_RTL, + NAVIGATION_STATE_LAND +} navigation_state_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -152,8 +158,7 @@ struct vehicle_status_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - unsigned int set_nav_state; /**< set navigation state machine to specified value */ - uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ + navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ failsafe_state_t failsafe_state; /**< current failsafe state */ -- cgit v1.2.3 From d41a01483a9a1e61c12492501bf975021595b3a6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 21 Apr 2014 22:48:12 +0200 Subject: fw_att_pos_estimator: lines were commented out by mistake --- src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index a61fff942..32a6029fe 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1035,8 +1035,8 @@ FixedwingEstimator::task_main() _local_pos.z_global = false; _local_pos.yaw = _att.yaw; - // _velocity_xy_filtered = 0.9f*_velocity_xy_filtered + 0.1f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); - // _velocity_z_filtered = 0.9f*_velocity_z_filtered + 0.1f*fabsf(_local_pos.vz); + _velocity_xy_filtered = 0.9f*_velocity_xy_filtered + 0.1f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); + _velocity_z_filtered = 0.9f*_velocity_z_filtered + 0.1f*fabsf(_local_pos.vz); /* crude land detector for fixedwing only, -- cgit v1.2.3 From c0d4ecf2b629a90222cf26d904c7b1260b1147c4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Apr 2014 11:42:46 +0200 Subject: navigator: some warnings and cleanup --- src/modules/navigator/navigator_main.cpp | 36 ++++++++++++++++---------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 34a28aec3..f39dc88e3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -159,8 +159,9 @@ private: vehicle_control_mode_s _control_mode; /**< vehicle control mode */ vehicle_global_position_s _global_pos; /**< global vehicle position */ home_position_s _home_pos; /**< home position for RTL */ - position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ mission_item_s _mission_item; /**< current mission item */ + navigation_capabilities_s _nav_caps; + position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ bool _mission_item_valid; @@ -172,7 +173,6 @@ private: bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ - struct navigation_capabilities_s _nav_caps; Mission _mission; @@ -307,11 +307,6 @@ private: */ bool check_mission_item_reached(); - /** - * Perform actions when current mission item reached. - */ - // void on_mission_item_reached(); - /** * Move to next waypoint */ @@ -352,7 +347,7 @@ Navigator *g_navigator; Navigator::Navigator() : -/* state machine transition table */ + /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), @@ -366,18 +361,27 @@ Navigator::Navigator() : _capabilities_sub(-1), _control_mode_sub(-1), _pos_sp_triplet_pub(-1), + _vstatus({}), + _control_mode({}), + _global_pos({}), + _home_pos({}), + _mission_item({}), + _nav_caps({}), + _pos_sp_triplet({}), + _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), + _geofence({}), _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), - _mission(), - _pos_sp_triplet({}), - _mission_item({}), - _mission_item_valid(false), + _mission({}), + _rtl({}), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _update_triplet(false) + _update_triplet(false), + _parameters({}), + _parameter_handles({}) { _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); @@ -530,12 +534,9 @@ Navigator::task_main() { /* inform about start */ warnx("Initializing.."); - fflush(stdout); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[navigator] started"); - /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager @@ -578,7 +579,6 @@ Navigator::task_main() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = NAV_STATE_NONE_ON_GROUND; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -811,7 +811,7 @@ Navigator::status() break; case NAV_STATE_LAND: - warnx("State: LAND"); + warnx("State: Land"); break; default: -- cgit v1.2.3 From 9f857f86e44a6385daa383d9b02173ad3c36fa01 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Apr 2014 12:24:52 +0200 Subject: navigator: corrected the RTL waypoint types --- src/modules/navigator/rtl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 6e621e39d..b8ea06275 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -119,7 +119,7 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss new_mission_item->yaw = NAN; new_mission_item->loiter_radius = _loiter_radius; new_mission_item->loiter_direction = 1; - new_mission_item->nav_cmd = NAV_CMD_TAKEOFF; + new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; new_mission_item->acceptance_radius = _acceptance_radius; new_mission_item->time_inside = 0.0f; new_mission_item->pitch_min = 0.0f; @@ -168,7 +168,7 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss new_mission_item->yaw = NAN; new_mission_item->loiter_radius = _loiter_radius; new_mission_item->loiter_direction = 1; - new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; + new_mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; new_mission_item->acceptance_radius = _acceptance_radius; new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); new_mission_item->pitch_min = 0.0f; @@ -250,4 +250,4 @@ RTL::move_to_next() default: break; } -} \ No newline at end of file +} -- cgit v1.2.3 From ea0142810a734bd8cade70668eefa562a495ec4a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Apr 2014 12:55:27 +0200 Subject: navigator: comments and whitespace --- src/modules/navigator/navigator_main.cpp | 94 +++++++++++--------------------- 1 file changed, 32 insertions(+), 62 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f39dc88e3..c3fc4e939 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -142,7 +142,7 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _navigator_task; /**< task handle for sensor task */ - int _mavlink_fd; + int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ int _global_pos_sub; /**< global position subscription */ int _home_pos_sub; /**< home position subscription */ @@ -160,30 +160,31 @@ private: vehicle_global_position_s _global_pos; /**< global vehicle position */ home_position_s _home_pos; /**< home position for RTL */ mission_item_s _mission_item; /**< current mission item */ - navigation_capabilities_s _nav_caps; + navigation_capabilities_s _nav_caps; /**< navigation capabilities */ position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ - bool _mission_item_valid; + bool _mission_item_valid; /**< flags if the current mission item is valid */ perf_counter_t _loop_perf; /**< loop performance counter */ - Geofence _geofence; - bool _geofence_violation_warning_sent; + Geofence _geofence; /**< class that handles the geofence */ + bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ - bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ + bool _fence_valid; /**< flag if fence is valid */ + bool _inside_fence; /**< vehicle is inside fence */ - Mission _mission; + Mission _mission; /**< class that handles the missions */ - RTL _rtl; + RTL _rtl; /**< class that handles RTL */ - bool _waypoint_position_reached; - bool _waypoint_yaw_reached; - uint64_t _time_first_inside_orbit; - MissionFeasibilityChecker missionFeasiblityChecker; + bool _waypoint_position_reached; /**< flags if the position of the mission item has been reached */ + bool _waypoint_yaw_reached; /**< flags if the yaw position of the mission item has been reached */ + uint64_t _time_first_inside_orbit; /**< the time when we were first in the acceptance radius of the mission item */ - bool _update_triplet; + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ + + bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ struct { float acceptance_radius; @@ -210,7 +211,7 @@ private: EVENT_HOME_POSITION_CHANGED, EVENT_MISSION_ITEM_REACHED, MAX_EVENT - }; + }; /**< possible events that can be thrown at state machine */ /** * State machine transition table @@ -267,8 +268,6 @@ private: */ void task_main(); - void publish_safepoints(unsigned points); - /** * Functions that are triggered when a new state is entered. */ @@ -283,22 +282,7 @@ private: bool start_land(); /** - * Fork for state transitions - */ - // void request_loiter_or_ready(); - - /** - * Guards offboard mission - */ - bool offboard_mission_available(unsigned relative_index); - - /** - * Guards onboard mission - */ - bool onboard_mission_available(unsigned relative_index); - - /** - * Reset all "reached" flags. + * Reset all "mission item reached" flags. */ void reset_reached(); @@ -308,27 +292,24 @@ private: bool check_mission_item_reached(); /** - * Move to next waypoint + * Set mission items by accessing the mission class. + * If failing, tell the state machine to loiter. */ bool set_mission_items(); /** - * Switch to next RTL state + * Set a RTL item by accessing the RTL class. + * If failing, tell the state machine to loiter. */ void set_rtl_item(); /** - * Set position setpoint for mission item + * Translate mission item to a position setpoint. */ void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); /** - * Helper function to get a takeoff item - */ - void get_takeoff_setpoint(position_setpoint_s *pos_sp); - - /** - * Publish a new mission item triplet for position controller + * Publish a new position setpoint triplet for position controllers */ void publish_position_setpoint_triplet(); }; @@ -608,13 +589,12 @@ Navigator::task_main() /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) { + /* timed out - periodic check for _task_should_exit, etc. */ continue; - } - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { + } else if (pret < 0) { + /* this is undesirable but not much we can do - might want to flag unhappy status */ warn("poll error %d, %d", pret, errno); continue; } @@ -651,16 +631,11 @@ Navigator::task_main() break; case NAVIGATION_STATE_RTL: - /* TODO: what is this for? */ - // if (!(_rtl_state == RTL_STATE_DESCEND && - // (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - // _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); - // } break; case NAVIGATION_STATE_LAND: - /* TODO: add this */ + /* TODO: add and test this */ // dispatch(EVENT_LAND_REQUESTED); break; @@ -781,6 +756,7 @@ Navigator::status() if (_fence_valid) { warnx("Geofence is valid"); + /* TODO: needed? */ // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); @@ -944,12 +920,6 @@ Navigator::start_auto_on_ground() _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; - - // if (_rtl_state != RTL_STATE_DESCEND) { - // reset RTL state if landed not at home - // _rtl_state = RTL_STATE_NONE; - // } - _update_triplet = true; return true; } @@ -1057,7 +1027,6 @@ Navigator::set_mission_items() bool Navigator::start_rtl() { - /* TODO check if RTL was successfull, if not we have a problem and need to dispatch something */ if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { _mission_item_valid = true; @@ -1070,8 +1039,9 @@ Navigator::start_rtl() _update_triplet = true; return true; } - dispatch(EVENT_LOITER_REQUESTED); + /* if RTL doesn't work, fallback to loiter */ + dispatch(EVENT_LOITER_REQUESTED); return false; } @@ -1189,7 +1159,7 @@ Navigator::check_mission_item_reached() return _vstatus.condition_landed; } - /* TODO count turns */ + /* TODO: count turns */ // if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || // _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || // _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && @@ -1242,7 +1212,7 @@ Navigator::check_mission_item_reached() /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); - if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */ + if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ _waypoint_yaw_reached = true; } -- cgit v1.2.3 From 13dfe0447ccfa4f75b551d02b5c979a6ade4c81a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:07:29 +0200 Subject: Send current local position estimate as well --- src/modules/commander/commander.cpp | 24 ++++++++++++++++-------- src/modules/uORB/topics/home_position.h | 5 ++++- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 352711734..dd2614c1d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -927,6 +927,14 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } + /* update local position estimate */ + orb_check(local_position_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + } + /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ bool eph_epv_good; @@ -952,6 +960,10 @@ int commander_thread_main(int argc, char *argv[]) home.lon = global_position.lon; home.alt = global_position.alt; + home.x = local_position.x; + home.y = local_position.y; + home.z = local_position.z; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); @@ -968,14 +980,6 @@ int commander_thread_main(int argc, char *argv[]) tune_positive(true); } - /* update local position estimate */ - orb_check(local_position_sub, &updated); - - if (updated) { - /* position changed */ - orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - } - /* update condition_local_position_valid and condition_local_altitude_valid */ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); @@ -1338,6 +1342,10 @@ int commander_thread_main(int argc, char *argv[]) home.lon = global_position.lon; home.alt = global_position.alt; + home.x = local_position.x; + home.y = local_position.y; + home.z = local_position.z; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 08d11abae..70071130d 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -59,10 +59,13 @@ struct home_position_s { uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude in meters */ + + float x; /**< X coordinate in meters */ + float y; /**< Y coordinate in meters */ + float z; /**< Z coordinate in meters */ }; /** -- cgit v1.2.3 From e882824ee15e0c5fff58c7f223ec7be181c7af8f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Apr 2014 23:31:15 +0200 Subject: eph and epv renaming, make this compile again --- src/drivers/gps/gps.cpp | 6 +++--- src/drivers/gps/mtk.cpp | 4 ++-- src/drivers/gps/ubx.cpp | 4 ++-- .../attitude_estimator_ekf_main.cpp | 4 ++-- .../fw_att_pos_estimator_main.cpp | 5 ++--- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- src/modules/mavlink/mavlink_receiver.cpp | 6 +++--- src/modules/navigator/navigator_main.cpp | 21 +++++++++++---------- .../position_estimator_inav_main.c | 12 ++++++------ src/modules/sdlog2/sdlog2.c | 4 ++-- src/modules/uORB/topics/vehicle_gps_position.h | 4 ++-- 11 files changed, 37 insertions(+), 37 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a902bdf2f..f05f4a409 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -282,8 +282,8 @@ GPS::task_main() _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 3.0f; - _report.epv_m = 7.0f; + _report.eph = 3.0f; + _report.epv = 7.0f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; @@ -446,7 +446,7 @@ GPS::print_info() warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type, _report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m); + warnx("eph: %.2fm, epv: %.2fm", _report.eph, _report.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index c90ecbe28..99f88fb8a 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -253,8 +253,8 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm _gps_position->fix_type = packet.fix_type; - _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit - _gps_position->epv_m = 0.0; //unknown in mtk custom mode + _gps_position->eph = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit + _gps_position->epv = 0.0; //unknown in mtk custom mode _gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad _gps_position->satellites_visible = packet.satellites; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 8a2afecb7..95965b60d 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -425,8 +425,8 @@ UBX::handle_message() _gps_position->lat = packet->lat; _gps_position->lon = packet->lon; _gps_position->alt = packet->height_msl; - _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + _gps_position->eph = (float)packet->hAcc * 1e-3f; // from mm to m + _gps_position->epv = (float)packet->vAcc * 1e-3f; // from mm to m _gps_position->timestamp_position = hrt_absolute_time(); _rate_count_lat_lon++; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index c61b6ff3f..ec679f1ae 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -398,7 +398,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds hrt_abstime vel_t = 0; bool vel_valid = false; - if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { + if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { vel_valid = true; if (gps_updated) { vel_t = gps.timestamp_velocity; @@ -407,7 +407,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { + } else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index e5435e843..f47531b26 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1247,7 +1247,6 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); - _global_pos.rel_alt = (-_local_pos.z); if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -1255,8 +1254,8 @@ FixedwingEstimator::task_main() _global_pos.yaw = _local_pos.yaw; - _global_pos.eph = _gps.eph_m; - _global_pos.epv = _gps.epv_m; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; _global_pos.timestamp = _local_pos.timestamp; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 27b1af046..67ded1230 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -541,8 +541,8 @@ protected: gps->lat, gps->lon, gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), + cm_uint16_from_m_float(gps->eph), + cm_uint16_from_m_float(gps->epv), gps->vel_m_s * 100.0f, _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, gps->satellites_visible); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 33a4fef12..95314d56f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -661,12 +661,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; - hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m - hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m + hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m hil_gps.timestamp_variance = timestamp; hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; + hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph; hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c3fc4e939..d45446e5f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -743,16 +743,17 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); - - if (_global_pos.global_valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); - warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); - warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", - (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); - warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); - } + /* TODO: add this again */ + // warnx("Global position is %svalid", _global_pos_valid ? "" : "in"); + + // if (_global_pos.global_valid) { + // warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); + // warnx("Altitude %5.5f meters, altitude above home %5.5f meters", + // (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + // warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", + // (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); + // warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); + // } if (_fence_valid) { warnx("Geofence is valid"); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 368424853..54c8a7d17 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -592,13 +592,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) { + if (gps.eph > max_eph_epv * 1.5f || gps.epv > max_eph_epv * 1.5f || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) { + if (gps.eph < max_eph_epv && gps.epv < max_eph_epv && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); @@ -673,8 +673,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } - w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); - w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph); + w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv); } } else { @@ -951,8 +951,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.yaw = local_pos.yaw; // TODO implement dead-reckoning - global_pos.eph = gps.eph_m; - global_pos.epv = gps.epv_m; + global_pos.eph = gps.eph; + global_pos.epv = gps.epv; if (vehicle_global_position_pub < 0) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b74d4183b..5d49cc4c9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -972,8 +972,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; - log_msg.body.log_GPS.eph = buf_gps_pos.eph_m; - log_msg.body.log_GPS.epv = buf_gps_pos.epv_m; + log_msg.body.log_GPS.eph = buf_gps_pos.eph; + log_msg.body.log_GPS.epv = buf_gps_pos.epv; log_msg.body.log_GPS.lat = buf_gps_pos.lat; log_msg.body.log_GPS.lon = buf_gps_pos.lon; log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f; diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 794c3f8bc..a75810278 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -65,8 +65,8 @@ struct vehicle_gps_position_s { float c_variance_rad; /**< course accuracy estimate rad */ uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - float eph_m; /**< GPS HDOP horizontal dilution of position in m */ - float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + float eph; /**< GPS HDOP horizontal dilution of position in m */ + float epv; /**< GPS VDOP horizontal dilution of position in m */ uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ float vel_m_s; /**< GPS ground speed (m/s) */ -- cgit v1.2.3 From a6f71f10d0664754e0b3c8ad2161dc81befb2ca9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 27 Apr 2014 02:24:33 +0200 Subject: fw_att_pos_estimator: some tuning for the land/in-air detector --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index f47531b26..fcbd90405 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -240,6 +240,7 @@ private: float _velocity_xy_filtered; float _velocity_z_filtered; + float _airspeed_filtered; /** * Update our local parameter cache. @@ -343,7 +344,8 @@ FixedwingEstimator::FixedwingEstimator() : _mavlink_fd(-1), _ekf(nullptr), _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f) + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f) { last_run = hrt_absolute_time(); @@ -1200,16 +1202,17 @@ FixedwingEstimator::task_main() _local_pos.z_global = false; _local_pos.yaw = _att.yaw; - _velocity_xy_filtered = 0.9f*_velocity_xy_filtered + 0.1f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); - _velocity_z_filtered = 0.9f*_velocity_z_filtered + 0.1f*fabsf(_local_pos.vz); + _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); + _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); + _airspeed_filtered = 0.95*_airspeed_filtered + + 0.05*_airspeed.true_airspeed_m_s; /* crude land detector for fixedwing only, * TODO: adapt so that it works for both, maybe move to another location */ - if (_velocity_xy_filtered < 2 - && _velocity_z_filtered < 2 - && _airspeed.true_airspeed_m_s < 10) { + if (_velocity_xy_filtered < 5 + && _velocity_z_filtered < 10 + && _airspeed_filtered < 10) { _local_pos.landed = true; } else { _local_pos.landed = false; -- cgit v1.2.3 From 721c90291c12405b4f4a6cf5ddc9ca8cec9f0077 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 27 Apr 2014 02:26:09 +0200 Subject: commander: some HIL commands and land detector cleanup --- src/modules/commander/commander.cpp | 130 +++++++++++++++--------------------- 1 file changed, 53 insertions(+), 77 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6f86e0c2f..13336736d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. * Author: Petri Tanskanen * Lorenz Meier * Thomas Gubler @@ -383,7 +383,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe { /* result of the command */ enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; - bool ret = false; /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components @@ -395,89 +394,73 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - uint8_t base_mode = (uint8_t) cmd->param1; - uint8_t custom_main_mode = (uint8_t) cmd->param2; - transition_result_t arming_res = TRANSITION_NOT_CHANGED; + uint8_t base_mode = (uint8_t) cmd->param1; + uint8_t custom_main_mode = (uint8_t) cmd->param2; - /* set HIL state */ - hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); - /* if HIL got enabled, reset battery status state */ - if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { - /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); - - if (arming_res != TRANSITION_DENIED) { - mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + /* if HIL got enabled, reset battery status state */ + if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { + /* reset the arming mode to disarmed */ + arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); + } - } else { - mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); - } - } + // Transition the arming state + transition_result_t arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - if (hil_ret == OK) - ret = true; + /* set main state */ + transition_result_t main_res = TRANSITION_DENIED; - // Transition the arming state - arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); - if (arming_res == TRANSITION_CHANGED) - ret = true; + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - /* set main state */ - transition_result_t main_res = TRANSITION_DENIED; + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); - if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { - /* use autopilot-specific mode */ - if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { - /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + } - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else { + /* use base mode */ + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); - } - - } else { - /* use base mode */ - if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); - - } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { - if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); - - } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { - /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); } } + } - if (main_res == TRANSITION_CHANGED) - ret = true; - - if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; - break; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + break; + } + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. // We use an float epsilon delta to test float equality. @@ -503,13 +486,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe status->set_nav_state = NAVIGATION_STATE_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE status->set_nav_state = NAVIGATION_STATE_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; } else { mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); @@ -525,7 +506,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; } else { /* reject parachute depoyment not armed */ @@ -981,12 +961,10 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); - static bool published_condition_landed_fw = false; if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; - published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes if (status.condition_landed) { mavlink_log_critical(mavlink_fd, "#audio: LANDED"); @@ -995,12 +973,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } - } else { - if (!published_condition_landed_fw) { - status.condition_landed = false; // Fixedwing does not have a landing detector currently - published_condition_landed_fw = true; - status_changed = true; - } } /* update battery status */ @@ -1265,6 +1237,8 @@ int commander_thread_main(int argc, char *argv[]) /* LAND not allowed, set TERMINATION state */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } + } else { + status.set_nav_state = NAVIGATION_STATE_MISSION; } } else { @@ -1284,7 +1258,9 @@ int commander_thread_main(int argc, char *argv[]) /* LAND not allowed, set TERMINATION state */ res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } - } + } else { + status.set_nav_state = NAVIGATION_STATE_RTL; + } } } else { -- cgit v1.2.3 From 470513d9bb67bc19bd0ac70d209c681dc321ddfb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 27 Apr 2014 11:48:03 +0200 Subject: make it compile again after merge --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a8b6eae1c..7b9a558b5 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -979,7 +979,7 @@ FixedwingEstimator::task_main() // struct home_position_s home; // orb_copy(ORB_ID(home_position), _home_sub, &home); - if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { + if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; @@ -1017,7 +1017,7 @@ FixedwingEstimator::task_main() mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m); + warnx("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph, (double)_gps.epv); _gps_initialized = true; @@ -1259,8 +1259,8 @@ FixedwingEstimator::task_main() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; - _global_pos.eph = _gps.eph_m; - _global_pos.epv = _gps.epv_m; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; } if (_local_pos.v_xy_valid) { -- cgit v1.2.3 From 96ddea082690cd826cee10c4c00c95b2f87caf90 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Apr 2014 12:45:02 +0200 Subject: Introduce debug option for mTECS and silence it as default --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 37 +++++++++++++++++++++------ src/modules/fw_pos_control_l1/mtecs/mTecs.h | 5 ++++ 2 files changed, 34 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 1fb3153e9..a4cf0bdf8 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -42,7 +42,7 @@ #include "mTecs.h" #include -#include +#include namespace fwPosctrl { @@ -66,7 +66,8 @@ mTecs::mTecs() : timestampLastIteration(hrt_absolute_time()), _firstIterationAfterReset(true), _dtCalculated(false), - _counter(0) + _counter(0), + _debug(false) { } @@ -82,8 +83,8 @@ void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alt float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); if (_counter % 10 == 0) { - warnx("***"); - warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + debug("***"); + debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); } updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode); } @@ -95,7 +96,7 @@ void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAn float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); if (_counter % 10 == 0) { - warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); } updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode); } @@ -130,9 +131,9 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; if (_counter % 10 == 0) { - warnx("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", + debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); - warnx("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", + debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); } @@ -161,7 +162,7 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh if (_counter % 10 == 0) { - warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", + debug("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", (double)_throttleSp, (double)_pitchSp, (double)flightPathAngleSp, (double)flightPathAngle, (double)accelerationLongitudinalSp, (double)airspeedDerivative); @@ -198,5 +199,25 @@ void mTecs::updateTimeMeasurement() } } +void mTecs::debug_print(const char *fmt, va_list args) +{ + fprintf(stderr, "%s: ", "[mtecs]"); + vfprintf(stderr, fmt, args); + + fprintf(stderr, "\n"); +} + +void mTecs::debug(const char *fmt, ...) { + + if (!_debug) { + return; + } + + va_list args; + + va_start(args, fmt); + debug_print(fmt, args); +} + } /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 393dd294d..fab4b161a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -121,6 +121,11 @@ protected: bool _dtCalculated; /**< True if dt has been calculated in this iteration */ int _counter; + bool _debug; ///< Set true to enable debug output + + + static void debug_print(const char *fmt, va_list args); + void debug(const char *fmt, ...); /* * Measure and update the time step dt if this was not already done in the current iteration -- cgit v1.2.3 From dd8bfc2a0b8e528f7e3c93e2dfba5b10bd37091d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 1 May 2014 13:53:47 +0200 Subject: mtecs: landing mode which limits pitch and as well throttle at the end of the landing --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 22 +++++-------- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 8 +++++ src/modules/fw_pos_control_l1/mtecs/mTecs.h | 10 ++++-- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 37 ++++++++++++++++++++++ 4 files changed, 60 insertions(+), 17 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 44afa43d8..5cdab10a1 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -362,12 +362,14 @@ private: /* * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter) + * XXX need to clean up/remove this function once mtecs fully replaces TECS */ void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, float throttle_cruise, bool climbout_mode, float climbout_pitch_min_rad, - const math::Vector<3> &ground_speed); + const math::Vector<3> &ground_speed, + fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL); }; @@ -958,7 +960,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(flare_curve_alt, calculate_target_airspeed(airspeed_land), eas2tas, flare_pitch_angle_rad, math::radians(15.0f), 0.0f, throttle_max, throttle_land, - false, flare_pitch_angle_rad, ground_speed); + false, flare_pitch_angle_rad, ground_speed, land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -1031,7 +1033,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), ground_speed); + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), ground_speed, fwPosctrl::mTecs::TECS_MODE_TAKEOFF); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); @@ -1355,24 +1357,16 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, float throttle_cruise, bool climbout_mode, float climbout_pitch_min_rad, - const math::Vector<3> &ground_speed) + const math::Vector<3> &ground_speed, + fwPosctrl::mTecs::tecs_mode mode) { if (_mTecs.getEnabled()) { - float flightPathAngle = 0.0f; float ground_speed_length = ground_speed.length(); if (ground_speed_length > FLT_EPSILON) { flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } - - if (!climbout_mode) { - /* Normal operation */ - _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_NORMAL); - } else { - /* Climbout/Takeoff mode requested */ - _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_TAKEOFF); - } - + _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode); } else { _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index a4cf0bdf8..b717429d3 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -63,6 +63,8 @@ mTecs::mTecs() : _BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), _BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"), _BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true), + _BlockOutputLimiterLandThrottle(this, "LND_THR"), + _BlockOutputLimiterLandPitch(this, "LND_PIT", true), timestampLastIteration(hrt_absolute_time()), _firstIterationAfterReset(true), _dtCalculated(false), @@ -148,6 +150,12 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh if (mode == TECS_MODE_TAKEOFF) { outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; + } else if (mode == TECS_MODE_LAND) { + // only limit pitch but do not limit throttle + outputLimiterPitch = &_BlockOutputLimiterLandPitch; + } else if (mode == TECS_MODE_LAND_THROTTLELIM) { + outputLimiterThrottle = &_BlockOutputLimiterLandThrottle; + outputLimiterPitch = &_BlockOutputLimiterLandPitch; } else if (mode == TECS_MODE_UNDERSPEED) { outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index fab4b161a..9ed233ba0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -61,7 +61,9 @@ public: typedef enum { TECS_MODE_NORMAL, TECS_MODE_UNDERSPEED, - TECS_MODE_TAKEOFF + TECS_MODE_TAKEOFF, + TECS_MODE_LAND, + TECS_MODE_LAND_THROTTLELIM } tecs_mode; /* @@ -111,8 +113,10 @@ protected: /* Output Limits in special modes */ BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits during takeoff */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in last phase)*/ + BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ /* Time measurements */ hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index fd501c17a..3a05f2c88 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -323,3 +323,40 @@ PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f); */ PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f); +/** + * Minimal throttle in landing mode (only last phase of landing) + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f); + +/** + * Maximal throttle in landing mode (only last phase of landing) + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f); + +/** + * Minimal pitch in landing mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f); + +/** + * Maximal pitch in landing mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f); -- cgit v1.2.3 From 12390d7281985b7e3b6649fc9889e2e60a48dad1 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 4 May 2014 11:19:26 -0700 Subject: WIP: Mavlink file server --- src/modules/mavlink/mavlink_ftp.cpp | 377 +++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_ftp.h | 211 +++++++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 7 + src/modules/mavlink/mavlink_receiver.h | 2 + src/modules/mavlink/module.mk | 3 +- 5 files changed, 599 insertions(+), 1 deletion(-) create mode 100644 src/modules/mavlink/mavlink_ftp.cpp create mode 100644 src/modules/mavlink/mavlink_ftp.h diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp new file mode 100644 index 000000000..4cb31640e --- /dev/null +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -0,0 +1,377 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +#include "mavlink_ftp.h" + +MavlinkFTP *MavlinkFTP::_server; + +MavlinkFTP * +MavlinkFTP::getServer() +{ + // XXX this really cries out for some locking... + if (_server == nullptr) { + _server = new MavlinkFTP; + } + return _server; +} + +MavlinkFTP::MavlinkFTP() +{ + // initialise the request freelist + dq_init(&_workFree); + sem_init(&_lock, 0, 1); + + // drop work entries onto the free list + for (unsigned i = 0; i < kRequestQueueSize; i++) { + _qFree(&_workBufs[i]); + } + +} + +void +MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel) +{ + // get a free request + auto req = _dqFree(); + + // if we couldn't get a request slot, just drop it + if (req != nullptr) { + + // decode the request + req->decode(channel, msg); + + // and queue it for the worker + work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); + } +} + +void +MavlinkFTP::_workerTrampoline(void *arg) +{ + auto req = reinterpret_cast(arg); + auto server = MavlinkFTP::getServer(); + + // call the server worker with the work item + server->_worker(req); +} + +void +MavlinkFTP::_worker(Request *req) +{ + auto hdr = req->header(); + ErrorCode errorCode = kErrNone; + uint32_t messageCRC; + + // basic sanity checks; must validate length before use + if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) { + errorCode = kErrNoRequest; + goto out; + } + + // check request CRC to make sure this is one of ours + messageCRC = hdr->crc32; + hdr->crc32 = 0; + if (crc32(req->data(), req->dataSize()) != messageCRC) { + errorCode = kErrNoRequest; + goto out; + } + + switch (hdr->opcode) { + case kCmdNone: + break; + + case kCmdTerminate: + if (!Session::terminate(hdr->session)) { + errorCode = kErrNoRequest; + } + break; + + case kCmdReset: + Session::reset(); + break; + + case kCmdList: + errorCode = _workList(req); + break; + + case kCmdOpen: + errorCode = _workOpen(req, false); + break; + + case kCmdCreate: + errorCode = _workOpen(req, true); + break; + + case kCmdRead: + errorCode = _workRead(req); + break; + + case kCmdWrite: + errorCode = _workWrite(req); + break; + + case kCmdRemove: + errorCode = _workRemove(req); + break; + + default: + errorCode = kErrNoRequest; + break; + } + +out: + // handle success vs. error + if (errorCode == kErrNone) { + hdr->opcode = kRspAck; + } else { + hdr->opcode = kRspNak; + hdr->size = 1; + hdr->data[0] = errorCode; + } + + // respond to the request + _reply(req); + + // free the request buffer back to the freelist + _qFree(req); +} + +void +MavlinkFTP::_reply(Request *req) +{ + auto hdr = req->header(); + + // message is assumed to be already constructed in the request buffer, so generate the CRC + hdr->crc32 = 0; + hdr->crc32 = crc32(req->data(), req->dataSize()); + + // then pack and send the reply back to the request source + mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->data()); +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workList(Request *req) +{ + auto hdr = req->header(); + + // open directory + + // seek in directory + + // read entries until buffer is full + + // send reply + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workOpen(Request *req, bool create) +{ + auto hdr = req->header(); + + // allocate session ID + int session = Session::allocate(); + if (session < 0) { + return kErrNoSession; + } + + // get the session to open the file + if (!Session::get(session)->open(req->dataAsCString(), create)) { + return create ? kErrPerm : kErrNotFile; + } + + // save the session ID in the reply + hdr->session = session; + hdr->size = 0; + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workRead(Request *req) +{ + auto hdr = req->header(); + + // look up session + auto session = Session::get(hdr->session); + if (session == nullptr) { + return kErrNoSession; + } + + // read from file + int result = session->read(hdr->offset, &hdr->data[0], hdr->size); + + if (result < 0) { + return kErrIO; + } + hdr->size = result; + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workWrite(Request *req) +{ + auto hdr = req->header(); + + // look up session + auto session = Session::get(hdr->session); + if (session == nullptr) { + return kErrNoSession; + } + + // append to file + int result = session->append(hdr->offset, &hdr->data[0], hdr->size); + + if (result < 0) { + // XXX might also be no space, I/O, etc. + return kErrNotAppend; + } + + hdr->size = result; + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemove(Request *req) +{ + auto hdr = req->header(); + + // for now, send error reply + return kErrPerm; +} + +MavlinkFTP::Session MavlinkFTP::Session::_sessions[MavlinkFTP::Session::kMaxSession]; + +int +MavlinkFTP::Session::allocate() +{ + for (unsigned i = 0; i < kMaxSession; i++) { + if (_sessions[i]._fd < 0) { + return i; + } + } + return -1; +} + +MavlinkFTP::Session * +MavlinkFTP::Session::get(unsigned index) +{ + if ((index >= kMaxSession) || (_sessions[index]._fd < 0)) { + return nullptr; + } + return &_sessions[index]; +} + +void +MavlinkFTP::Session::terminate() +{ + // clean up aborted transfers? + if (_fd >= 0) { + close(_fd); + _fd = -1; + } +} + +bool +MavlinkFTP::Session::terminate(unsigned index) + { + Session *session = get(index); + + if (session == nullptr) { + return false; + } + + session->terminate(); + return true; +} + +void +MavlinkFTP::Session::reset() +{ + for (unsigned i = 0; i < kMaxSession; i++) { + terminate(i); + } +} + +bool +MavlinkFTP::Session::open(const char *path, bool create) +{ + int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; + + _fd = open(path, oflag); + if (_fd < 0) { + return false; + } + return true; +} + +int +MavlinkFTP::Session::read(off_t offset, uint8_t *buf, uint8_t count) +{ + // can we seek to the location? + if (lseek(_fd, offset, SEEK_SET) < 0) { + return -1; + } + + return read(_fd, buf, count); +} + +int +MavlinkFTP::Session::append(off_t offset, uint8_t *buf, uint8_t count) +{ + // make sure that the requested offset matches our current position + off_t pos = lseek(_fd, 0, SEEK_CUR); + if (pos != offset) { + return -1; + } + return write(_fd, buf, count); +} + +char * +MavlinkFTP::Request::dataAsCString() +{ + // guarantee nul termination + if (header()->size < kMaxDataLength) { + data()[header()->size] = '\0'; + } else { + data()[kMaxDataLength - 1] = '\0'; + } + + // and return data + return (char *)data(); +} diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h new file mode 100644 index 000000000..a4f67793e --- /dev/null +++ b/src/modules/mavlink/mavlink_ftp.h @@ -0,0 +1,211 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +/** + * @file mavlink_ftp.h + * + * MAVLink remote file server. + * + * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes + * a session ID and sequence number. + * + * A limited number of requests (currently 2) may be outstanding at a time. + * Additional messages will be discarded. + * + * Messages consist of a fixed header, followed by a data area. + * + */ + +#include +#include + +#include + +#include "mavlink_messages.h" + +class MavlinkFTP +{ +public: + MavlinkFTP(); + + static MavlinkFTP *getServer(); + + // static interface + void handle_message(mavlink_message_t *msg, + mavlink_channel_t channel); + +private: + + static const unsigned kRequestQueueSize = 2; + + static MavlinkFTP *_server; + + struct RequestHeader + { + uint8_t magic; + uint8_t session; + uint8_t opcode; + uint8_t size; + uint32_t crc32; + uint32_t offset; + uint8_t data[]; + }; + + struct FileList + { + uint32_t fileSize; + uint8_t nameLength; + uint8_t name[]; + }; + + enum Opcode : uint8_t + { + kCmdNone, // ignored, always acked + kCmdTerminate, // releases sessionID, closes file + kCmdReset, // terminates all sessions + kCmdList, // list files in from + kCmdOpen, // opens for reading, returns + kCmdRead, // reads bytes from in + kCmdCreate, // creates for writing, returns + kCmdWrite, // appends bytes at in + kCmdRemove, // remove file (only if created by server?) + + kRspAck, + kRspNak + }; + + enum ErrorCode : uint8_t + { + kErrNone, + kErrNoRequest, + kErrNoSession, + kErrSequence, + kErrNotDir, + kErrNotFile, + kErrEOF, + kErrNotAppend, + kErrTooBig, + kErrIO, + kErrPerm + }; + + class Session + { + public: + Session() : _fd(-1) {} + + static int allocate(); + static Session *get(unsigned index); + static bool terminate(unsigned index); + static void reset(); + + void terminate(); + bool open(const char *path, bool create); + int read(off_t offset, uint8_t *buf, uint8_t count); + int append(off_t offset, uint8_t *buf, uint8_t count); + + private: + static const unsigned kMaxSession = 2; + static Session _sessions[kMaxSession]; + + int _fd; + }; + + class Request + { + public: + union { + dq_entry_t entry; + work_s work; + }; + mavlink_channel_t channel; + + void decode(mavlink_channel_t fromChannel, mavlink_message_t *fromMessage) { + channel = fromChannel; + mavlink_msg_encapsulated_data_decode(fromMessage, &_message); + } + + RequestHeader *header() { return reinterpret_cast(&_message.data[0]); } + uint8_t *data() { return &_message.data[0]; } + unsigned dataSize() { return header()->size + sizeof(RequestHeader); } + uint16_t sequence() const { return _message.seqnr; } + + char *dataAsCString(); + + private: + mavlink_encapsulated_data_t _message; + + }; + + static const uint8_t kProtocolMagic = 'f'; + static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader); + + /// Request worker; runs on the low-priority work queue to service + /// remote requests. + /// + static void _workerTrampoline(void *arg); + void _worker(Request *req); + + /// Reply to a request (XXX should be a Request method) + /// + void _reply(Request *req); + + ErrorCode _workList(Request *req); + ErrorCode _workOpen(Request *req, bool create); + ErrorCode _workRead(Request *req); + ErrorCode _workWrite(Request *req); + ErrorCode _workRemove(Request *req); + + // work freelist + Request _workBufs[kRequestQueueSize]; + dq_queue_t _workFree; + sem_t _lock; + + void _qLock() { do {} while (sem_wait(&_lock) != 0); } + void _qUnlock() { sem_post(&_lock); } + + void _qFree(Request *req) { + _qLock(); + dq_addlast(&req->entry, &_workFree); + _qUnlock(); + } + + Request *_dqFree() { + _qLock(); + auto req = reinterpret_cast(dq_remfirst(&_workFree)); + _qUnlock(); + return req; + } +}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7c93c1c00..fd1abe5ee 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -112,6 +112,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_alt0(0.0) { memset(&hil_local_pos, 0, sizeof(hil_local_pos)); + + // make sure the FTP server is started + (void)MavlinkFTP::getServer(); } MavlinkReceiver::~MavlinkReceiver() @@ -150,6 +153,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_manual_control(msg); break; + case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + MavlinkFTP::getServer()->handle_message(msg, _mavlink->get_channel()); + break; + default: break; } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..36e6143ac 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -68,6 +68,8 @@ #include #include +#include "mavlink_ftp.h" + class Mavlink; class MavlinkReceiver diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index dcca11977..c348a33db 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -43,7 +43,8 @@ SRCS += mavlink_main.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ mavlink_rate_limiter.cpp \ - mavlink_commands.cpp + mavlink_commands.cpp \ + mavlink_ftp.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink -- cgit v1.2.3 From ef7c57f1cece0e49cd95e7bbdc0d6563eca6a9eb Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 4 May 2014 12:25:25 -0700 Subject: Implement directory listing --- src/modules/mavlink/mavlink_ftp.cpp | 51 +++++++++++++++++++++++++++++++++---- src/modules/mavlink/mavlink_ftp.h | 10 +++----- 2 files changed, 49 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 4cb31640e..16f96f2cc 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -187,16 +187,57 @@ MavlinkFTP::ErrorCode MavlinkFTP::_workList(Request *req) { auto hdr = req->header(); + DIR *dp = opendir(req->dataAsCString()); - // open directory + if (dp == nullptr) { + return kErrNotDir; + } - // seek in directory + ErrorCode errorCode = kErrNone; + struct dirent entry, *result = nullptr; + unsigned offset = 0; - // read entries until buffer is full + // move to the requested offset + seekdir(dp, hdr->offset); - // send reply + for (;;) { + // read the directory entry + if (readdir_r(dp, &entry, &result)) { + errorCode = kErrIO; + break; + } - return kErrNone; + // no more entries? + if (result == nullptr) { + break; + } + + // name too big to fit? + if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) { + break; + } + + // store the type marker + switch (entry.d_type) { + case DTYPE_FILE: + hdr->data[offset++] = kDirentFile; + break; + case DTYPE_DIRECTORY: + hdr->data[offset++] = kDirentDir; + break; + default: + hdr->data[offset++] = kDirentUnknown; + break; + } + + // copy the name, which we know will fit + strcpy((char *)&hdr->data[offset], entry.d_name); + } + + closedir(dp); + hdr->size = offset; + + return errorCode; } MavlinkFTP::ErrorCode diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index a4f67793e..9615f7200 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -83,13 +83,6 @@ private: uint8_t data[]; }; - struct FileList - { - uint32_t fileSize; - uint8_t nameLength; - uint8_t name[]; - }; - enum Opcode : uint8_t { kCmdNone, // ignored, always acked @@ -170,6 +163,9 @@ private: }; static const uint8_t kProtocolMagic = 'f'; + static const char kDirentFile = 'F'; + static const char kDirentDir = 'D'; + static const char kDirentUnknown = 'U'; static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader); /// Request worker; runs on the low-priority work queue to service -- cgit v1.2.3 From 6351fd1e2cf9e2f7448558b3516ce84a988ff3da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 May 2014 13:48:05 +0200 Subject: Added debug printfs --- src/modules/mavlink/mavlink_ftp.h | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 9615f7200..eab2a567a 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -52,6 +52,7 @@ #include #include +#include #include "mavlink_messages.h" @@ -146,8 +147,16 @@ private: mavlink_channel_t channel; void decode(mavlink_channel_t fromChannel, mavlink_message_t *fromMessage) { - channel = fromChannel; - mavlink_msg_encapsulated_data_decode(fromMessage, &_message); + switch (fromMessage->msgid) { + + case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + channel = fromChannel; + mavlink_msg_encapsulated_data_decode(fromMessage, &_message); + warnx("got enc data"); + break; + default: + warnx("unknown msg"); + } } RequestHeader *header() { return reinterpret_cast(&_message.data[0]); } -- cgit v1.2.3 From 68352cb923d366b66bb68c8d946c4960b6f7ff1a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 7 May 2014 23:53:27 +0200 Subject: merge fixes --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 35ba96f59..dadcc1dce 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1029,7 +1029,7 @@ FixedwingEstimator::task_main() float initVelNED[3]; - if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { + if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { initVelNED[0] = _gps.vel_n_m_s; initVelNED[1] = _gps.vel_e_m_s; -- cgit v1.2.3 From 559c62b6bc4923854e106be5987db92197e0bae1 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Fri, 9 May 2014 15:17:38 -0700 Subject: Changed low threshold in px4io firmware to 10% to ensure compatibility with user configured single channel, mode switches --- src/modules/px4iofirmware/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 56c5aa005..a8c560d40 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -47,8 +47,8 @@ #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ -#define RC_CHANNEL_HIGH_THRESH 5000 -#define RC_CHANNEL_LOW_THRESH -5000 +#define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */ +#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); -- cgit v1.2.3 From bd9d58f565383598db785908a7cc08f87f6a07f1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 12 May 2014 23:06:45 +0200 Subject: attitude_estimator_ekf: auto detect mag declination using GPS coordinates --- src/lib/geo/geo.h | 2 +- .../attitude_estimator_ekf_main.cpp | 26 +++++++++++++++++----- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index e2f3da6f8..87c1cf460 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -50,7 +50,7 @@ __BEGIN_DECLS -#include "geo/geo_mag_declination.h" +#include "geo_mag_declination.h" #include diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index c61b6ff3f..e10c7de56 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -64,6 +64,7 @@ #include #include +#include #include #include @@ -292,6 +293,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; + /* magnetic declination, in radians */ + float mag_decl = 0.0f; + /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; R_decl.identity(); @@ -330,9 +334,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); - - /* update mag declination rotation matrix */ - R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); } /* only run filter if sensor values changed */ @@ -345,6 +346,13 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_check(sub_gps, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); + + if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); + + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, mag_decl); + } } bool global_pos_updated; @@ -474,7 +482,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ - R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); + if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); + + } else { + mag_decl = ekf_params.mag_decl; + } + + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, mag_decl); x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; @@ -522,7 +538,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.roll = euler[0]; att.pitch = euler[1]; - att.yaw = euler[2] + ekf_params.mag_decl; + att.yaw = euler[2] + mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; -- cgit v1.2.3 From 4d7cb184dbb94ca8b1747811de84de965a2f007f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 14 May 2014 18:19:07 +0200 Subject: mtecs: change main functions to int and add some comments --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 23 ++++++++++++++++++----- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 6 +++--- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index b717429d3..5dba0dcd6 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -77,33 +77,43 @@ mTecs::~mTecs() { } -void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode) +int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode) { /* time measurement */ updateTimeMeasurement(); + /* calculate flight path angle setpoint from altitude setpoint */ float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); + + /* Debug output */ if (_counter % 10 == 0) { debug("***"); debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); } - updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode); + + /* use flightpath angle setpoint for total energy control */ + return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode); } -void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode) { +int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode) { /* time measurement */ updateTimeMeasurement(); + /* calculate longitudinal acceleration setpoint from airspeed setpoint*/ float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); + + /* Debug output */ if (_counter % 10 == 0) { debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); } - updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode); + + /* use longitudinal acceleration setpoint for total energy control */ + return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode); } -void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode) +int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode) { /* time measurement */ updateTimeMeasurement(); @@ -132,6 +142,7 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh float energyDistributionRateSp = flightPathAngleSp - airspeedSpDerivativeNorm; float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; + /* Debug output */ if (_counter % 10 == 0) { debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); @@ -182,6 +193,8 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh _dtCalculated = false; _counter++; + + return 0; } void mTecs::resetIntegrators() diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 9ed233ba0..147c108f3 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -69,17 +69,17 @@ public: /* * Control in altitude setpoint and speed mode */ - void updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode); + int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode */ - void updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode); + int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) */ - void updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode); + int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode); /* * Reset all integrators -- cgit v1.2.3 From 5d04bb74cbee6e57db4e9b09c02139e1df6954d1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 14 May 2014 21:54:59 +0200 Subject: mtecs: check if input arguments are finite --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 5dba0dcd6..c6301bcdb 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -79,6 +79,11 @@ mTecs::~mTecs() int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode) { + /* check if all input arguments are numbers and abort if not so */ + if (!isfinite(flightPathAngle) || !isfinite(altitude) || + !isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + return -1; + } /* time measurement */ updateTimeMeasurement(); @@ -96,7 +101,13 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode); } -int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode) { +int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode) +{ + /* check if all input arguments are numbers and abort if not so */ + if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || + !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + return -1; + } /* time measurement */ updateTimeMeasurement(); @@ -115,6 +126,11 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode) { + /* check if all input arguments are numbers and abort if not so */ + if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || + !isfinite(airspeed) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { + return -1; + } /* time measurement */ updateTimeMeasurement(); -- cgit v1.2.3 From fbb3adde06e5ecf88a4c39e332a539fa12d173b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 16:04:02 +0200 Subject: mavlink app: Clean up allocations --- src/modules/mavlink/mavlink_commands.cpp | 29 +- src/modules/mavlink/mavlink_main.cpp | 112 +- src/modules/mavlink/mavlink_main.h | 10 +- src/modules/mavlink/mavlink_messages.cpp | 1928 +++++++++++----------- src/modules/mavlink/mavlink_messages.h | 15 +- src/modules/mavlink/mavlink_orb_subscription.cpp | 19 +- src/modules/mavlink/mavlink_orb_subscription.h | 18 +- src/modules/mavlink/mavlink_receiver.cpp | 1 - src/modules/mavlink/mavlink_stream.h | 22 +- 9 files changed, 1080 insertions(+), 1074 deletions(-) diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp index 1c1e097a4..5760d7512 100644 --- a/src/modules/mavlink/mavlink_commands.cpp +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -43,7 +43,6 @@ MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel) { _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); - _cmd = (struct vehicle_command_s *)_cmd_sub->get_data(); } MavlinkCommandsStream::~MavlinkCommandsStream() @@ -53,21 +52,23 @@ MavlinkCommandsStream::~MavlinkCommandsStream() void MavlinkCommandsStream::update(const hrt_abstime t) { - if (_cmd_sub->update(t)) { + struct vehicle_command_s cmd; + + if (_cmd_sub->update(t, &cmd)) { /* only send commands for other systems/components */ - if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) { + if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { mavlink_msg_command_long_send(_channel, - _cmd->target_system, - _cmd->target_component, - _cmd->command, - _cmd->confirmation, - _cmd->param1, - _cmd->param2, - _cmd->param3, - _cmd->param4, - _cmd->param5, - _cmd->param6, - _cmd->param7); + cmd.target_system, + cmd.target_component, + cmd.command, + cmd.confirmation, + cmd.param1, + cmd.param2, + cmd.param3, + cmd.param4, + cmd.param5, + cmd.param6, + cmd.param7); } } } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6c97bfca7..340b20e1b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -234,6 +234,11 @@ Mavlink::Mavlink() : _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), _message_buffer({}), + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -493,44 +498,39 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) void Mavlink::mavlink_update_system(void) { - static bool initialized = false; - static param_t param_system_id; - static param_t param_component_id; - static param_t param_system_type; - static param_t param_use_hil_gps; - if (!initialized) { - param_system_id = param_find("MAV_SYS_ID"); - param_component_id = param_find("MAV_COMP_ID"); - param_system_type = param_find("MAV_TYPE"); - param_use_hil_gps = param_find("MAV_USEHILGPS"); - initialized = true; + if (!_param_initialized) { + _param_system_id = param_find("MAV_SYS_ID"); + _param_component_id = param_find("MAV_COMP_ID"); + _param_system_type = param_find("MAV_TYPE"); + _param_use_hil_gps = param_find("MAV_USEHILGPS"); + _param_initialized = true; } /* update system and component id */ int32_t system_id; - param_get(param_system_id, &system_id); + param_get(_param_system_id, &system_id); if (system_id > 0 && system_id < 255) { mavlink_system.sysid = system_id; } int32_t component_id; - param_get(param_component_id, &component_id); + param_get(_param_component_id, &component_id); if (component_id > 0 && component_id < 255) { mavlink_system.compid = component_id; } int32_t system_type; - param_get(param_system_type, &system_type); + param_get(_param_system_type, &system_type); if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { mavlink_system.type = system_type; } int32_t use_hil_gps; - param_get(param_use_hil_gps, &use_hil_gps); + param_get(_param_use_hil_gps, &use_hil_gps); _use_hil_gps = (bool)use_hil_gps; } @@ -791,7 +791,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* Start sending parameters */ mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + mavlink_missionlib_send_gcs_string("[pm] sending list"); } break; case MAVLINK_MSG_ID_PARAM_SET: { @@ -813,7 +813,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[mavlink pm] unknown: %s", name); + sprintf(buf, "[pm] unknown: %s", name); mavlink_missionlib_send_gcs_string(buf); } else { @@ -1001,8 +1001,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) } else { mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - - if (_verbose) { warnx("ERROR: index out of bounds"); } } } @@ -1073,8 +1071,6 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u } else { mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - - if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); } } } @@ -1105,8 +1101,6 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) mavlink_missionlib_send_gcs_string("Operation timeout"); - if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; _wpm->current_partner_sysid = 0; _wpm->current_partner_compid = 0; @@ -1137,8 +1131,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); } } break; @@ -1162,21 +1154,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - - if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); } } } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - - if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } - } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); } } break; @@ -1206,14 +1191,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - - if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } } } else { mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - - if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); } } break; @@ -1230,8 +1211,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); } - break; } @@ -1242,15 +1221,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpr.seq == 0) { - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - if (_verbose) { warnx("REJ. WP CMD: First id != 0"); } - break; } @@ -1258,17 +1235,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq == _wpm->current_wp_id) { - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } } else if (wpr.seq == _wpm->current_wp_id + 1) { - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); } - break; } @@ -1276,8 +1251,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); } - break; } @@ -1291,7 +1264,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } + mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds"); } @@ -1301,13 +1274,9 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } - } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } @@ -1331,15 +1300,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } if (wpc.count == 0) { - mavlink_missionlib_send_gcs_string("COUNT 0"); - - if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); } + mavlink_missionlib_send_gcs_string("WP COUNT 0"); break; } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; _wpm->current_wp_id = 0; _wpm->current_partner_sysid = msg->sysid; @@ -1353,25 +1318,17 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_wp_id == 0) { mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP"); } } else { mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - - if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } } } else { mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } break; @@ -1393,7 +1350,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wp.seq != 0) { mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); break; } @@ -1401,12 +1357,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wp.seq >= _wpm->current_count) { mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); break; } if (wp.seq != _wpm->current_wp_id) { - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id); + mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch"); mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); break; } @@ -1473,8 +1428,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1515,8 +1468,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1535,8 +1486,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - - mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len); + mavlink_send_uart_bytes(_channel, buf, len); } @@ -1619,6 +1569,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) if (interval > 0) { /* search for stream with specified name in supported streams list */ for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { /* create new instance */ stream = streams_list[i]->new_instance(); @@ -1924,7 +1875,7 @@ Mavlink::task_main(int argc, char *argv[]) /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ if (_passing_on) { /* initialize message buffer if multiplexing is on */ - if (OK != message_buffer_init(500)) { + if (OK != message_buffer_init(300)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1956,7 +1907,8 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); - struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); + struct vehicle_status_s status; + status_sub->update(0, &status); MavlinkCommandsStream commands_stream(this, _channel); @@ -2013,14 +1965,14 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); - if (param_sub->update(t)) { + if (param_sub->update(t, nullptr)) { /* parameters updated */ mavlink_update_system(); } - if (status_sub->update(t)) { + if (status_sub->update(t, &status)) { /* switch HIL mode if required */ - set_hil_enabled(status->hil_state == HIL_STATE_ON); + set_hil_enabled(status.hil_state == HIL_STATE_ON); } /* update commands stream */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c7a7d32f8..1f0445cb6 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -278,11 +278,15 @@ private: int size; char *data; }; - mavlink_message_buffer _message_buffer; - - pthread_mutex_t _message_buffer_mutex; + mavlink_message_buffer _message_buffer; + pthread_mutex_t _message_buffer_mutex; + bool _param_initialized; + param_t _param_system_id; + param_t _param_component_id; + param_t _param_system_type; + param_t _param_use_hil_gps; /** * Send one parameter. diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 79dd88657..4bb827116 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -74,6 +74,8 @@ #include #include +#include + #include "mavlink_messages.h" @@ -189,42 +191,51 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set class MavlinkStreamHeartbeat : public MavlinkStream { public: - const char *get_name() + + ~MavlinkStreamHeartbeat() {}; + + const char *get_name() const + { + return MavlinkStreamHeartbeat::get_name_static(); + } + + static const char *get_name_static() { return "HEARTBEAT"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamHeartbeat(); } private: MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct position_setpoint_triplet_s *pos_sp_triplet; + protected: + + explicit MavlinkStreamHeartbeat() {}; + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - status = (struct vehicle_status_s *)status_sub->get_data(); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } void send(const hrt_abstime t) { - (void)status_sub->update(t); - (void)pos_sp_triplet_sub->update(t); + struct vehicle_status_s status; + struct position_setpoint_triplet_s pos_sp_triplet; + + (void)status_sub->update(t, &status); + (void)pos_sp_triplet_sub->update(t, &pos_sp_triplet); uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); mavlink_msg_heartbeat_send(_channel, mavlink_system.type, @@ -240,12 +251,19 @@ protected: class MavlinkStreamSysStatus : public MavlinkStream { public: - const char *get_name() + ~MavlinkStreamSysStatus() {}; + + const char *get_name() const + { + return MavlinkStreamSysStatus::get_name_static(); + } + + static const char *get_name_static () { return "SYS_STATUS"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamSysStatus(); } @@ -255,29 +273,31 @@ private: struct vehicle_status_s *status; protected: + explicit MavlinkStreamSysStatus() {}; + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - status = (struct vehicle_status_s *)status_sub->get_data(); } void send(const hrt_abstime t) { - status_sub->update(t); + struct vehicle_status_s status; + (void)status_sub->update(t, &status); mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining * 100.0f, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); + status.onboard_control_sensors_present, + status.onboard_control_sensors_enabled, + status.onboard_control_sensors_health, + status.load * 1000.0f, + status.battery_voltage * 1000.0f, + status.battery_current * 1000.0f, + status.battery_remaining * 100.0f, + status.drop_rate_comm, + status.errors_comm, + status.errors_count1, + status.errors_count2, + status.errors_count3, + status.errors_count4); } }; @@ -285,23 +305,25 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: - MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0) + ~MavlinkStreamHighresIMU(); + + const char *get_name() const { + return MavlinkStreamHighresIMU::get_name_static(); } - const char *get_name() + static const char *get_name_static() { return "HIGHRES_IMU"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamHighresIMU(); } private: MavlinkOrbSubscription *sensor_sub; - struct sensor_combined_s *sensor; uint64_t accel_timestamp; uint64_t gyro_timestamp; @@ -309,48 +331,52 @@ private: uint64_t baro_timestamp; protected: + explicit MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0) + { + } + void subscribe(Mavlink *mavlink) { sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); - sensor = (struct sensor_combined_s *)sensor_sub->get_data(); } void send(const hrt_abstime t) { - if (sensor_sub->update(t)) { + struct sensor_combined_s sensor; + if (sensor_sub->update(t, &sensor)) { uint16_t fields_updated = 0; - if (accel_timestamp != sensor->accelerometer_timestamp) { + if (accel_timestamp != sensor.accelerometer_timestamp) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_timestamp = sensor->accelerometer_timestamp; + accel_timestamp = sensor.accelerometer_timestamp; } - if (gyro_timestamp != sensor->timestamp) { + if (gyro_timestamp != sensor.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_timestamp = sensor->timestamp; + gyro_timestamp = sensor.timestamp; } - if (mag_timestamp != sensor->magnetometer_timestamp) { + if (mag_timestamp != sensor.magnetometer_timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_timestamp = sensor->magnetometer_timestamp; + mag_timestamp = sensor.magnetometer_timestamp; } - if (baro_timestamp != sensor->baro_timestamp) { + if (baro_timestamp != sensor.baro_timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_timestamp = sensor->baro_timestamp; + baro_timestamp = sensor.baro_timestamp; } mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, + sensor.timestamp, + sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], + sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], + sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], + sensor.baro_pres_mbar, sensor.differential_pressure_pa, + sensor.baro_alt_meter, sensor.baro_temp_celcius, fields_updated); } } @@ -360,12 +386,17 @@ protected: class MavlinkStreamAttitude : public MavlinkStream { public: - const char *get_name() + const char *get_name() const + { + return MavlinkStreamAttitude::get_name_static(); + } + + static const char *get_name_static() { return "ATTITUDE"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamAttitude(); } @@ -378,16 +409,17 @@ protected: void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - att = (struct vehicle_attitude_s *)att_sub->get_data(); } void send(const hrt_abstime t) { - if (att_sub->update(t)) { + struct vehicle_attitude_s att; + + if (att_sub->update(t, &att)) { mavlink_msg_attitude_send(_channel, - att->timestamp / 1000, - att->roll, att->pitch, att->yaw, - att->rollspeed, att->pitchspeed, att->yawspeed); + att.timestamp / 1000, + att.roll, att.pitch, att.yaw, + att.rollspeed, att.pitchspeed, att.yawspeed); } } }; @@ -396,39 +428,44 @@ protected: class MavlinkStreamAttitudeQuaternion : public MavlinkStream { public: - const char *get_name() + const char *get_name() const + { + return MavlinkStreamAttitudeQuaternion::get_name_static(); + } + + static const char *get_name_static() { return "ATTITUDE_QUATERNION"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamAttitudeQuaternion(); } private: MavlinkOrbSubscription *att_sub; - struct vehicle_attitude_s *att; protected: void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - att = (struct vehicle_attitude_s *)att_sub->get_data(); } void send(const hrt_abstime t) { - if (att_sub->update(t)) { + struct vehicle_attitude_s att; + + if (att_sub->update(t, &att)) { mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + att.timestamp / 1000, + att.q[0], + att.q[1], + att.q[2], + att.q[3], + att.rollspeed, + att.pitchspeed, + att.yawspeed); } } }; @@ -437,12 +474,18 @@ protected: class MavlinkStreamVFRHUD : public MavlinkStream { public: - const char *get_name() + + const char *get_name() const + { + return MavlinkStreamVFRHUD::get_name_static(); + } + + static const char *get_name_static() { return "VFR_HUD"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamVFRHUD(); } @@ -467,41 +510,38 @@ protected: void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - att = (struct vehicle_attitude_s *)att_sub->get_data(); - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - pos = (struct vehicle_global_position_s *)pos_sub->get_data(); - armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); - armed = (struct actuator_armed_s *)armed_sub->get_data(); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - act = (struct actuator_controls_s *)act_sub->get_data(); - airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); - airspeed = (struct airspeed_s *)airspeed_sub->get_data(); } void send(const hrt_abstime t) { - bool updated = att_sub->update(t); - updated |= pos_sub->update(t); - updated |= armed_sub->update(t); - updated |= act_sub->update(t); - updated |= airspeed_sub->update(t); + struct vehicle_attitude_s att; + struct vehicle_global_position_s pos; + struct actuator_armed_s armed; + struct actuator_controls_s act; + struct airspeed_s airspeed; + + bool updated = att_sub->update(t, &att); + updated |= pos_sub->update(t, &pos); + updated |= armed_sub->update(t, &armed); + updated |= act_sub->update(t, &act); + updated |= airspeed_sub->update(t, &airspeed); if (updated) { - float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); - uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; - float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; + float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); + uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; + float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; mavlink_msg_vfr_hud_send(_channel, - airspeed->true_airspeed_m_s, + airspeed.true_airspeed_m_s, groundspeed, heading, throttle, - pos->alt, - -pos->vel_d); + pos.alt, + pos.vel_d); } } }; @@ -510,12 +550,17 @@ protected: class MavlinkStreamGPSRawInt : public MavlinkStream { public: - const char *get_name() + const char *get_name() const + { + return MavlinkStreamGPSRawInt::get_name_static(); + } + + static const char *get_name_static() { return "GPS_RAW_INT"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamGPSRawInt(); } @@ -528,864 +573,865 @@ protected: void subscribe(Mavlink *mavlink) { gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); - gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (gps_sub->update(t)) { - mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); - } - } -}; - - -class MavlinkStreamGlobalPositionInt : public MavlinkStream -{ -public: - const char *get_name() - { - return "GLOBAL_POSITION_INT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamGlobalPositionInt(); - } - -private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_global_position_s *pos; - - MavlinkOrbSubscription *home_sub; - struct home_position_s *home; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - pos = (struct vehicle_global_position_s *)pos_sub->get_data(); - - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - home = (struct home_position_s *)home_sub->get_data(); - } - - void send(const hrt_abstime t) - { - bool updated = pos_sub->update(t); - updated |= home_sub->update(t); - - if (updated) { - mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); - } - } -}; - - -class MavlinkStreamLocalPositionNED : public MavlinkStream -{ -public: - const char *get_name() - { - return "LOCAL_POSITION_NED"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamLocalPositionNED(); - } - -private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_local_position_s *pos; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); - pos = (struct vehicle_local_position_s *)pos_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sub->update(t)) { - mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); - } - } -}; - - - -class MavlinkStreamViconPositionEstimate : public MavlinkStream -{ -public: - const char *get_name() - { - return "VICON_POSITION_ESTIMATE"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamViconPositionEstimate(); - } - -private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_vicon_position_s *pos; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sub->update(t)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->roll, - pos->pitch, - pos->yaw); - } - } -}; - - -class MavlinkStreamGPSGlobalOrigin : public MavlinkStream -{ -public: - const char *get_name() - { - return "GPS_GLOBAL_ORIGIN"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamGPSGlobalOrigin(); - } - -private: - MavlinkOrbSubscription *home_sub; - struct home_position_s *home; - -protected: - void subscribe(Mavlink *mavlink) - { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - home = (struct home_position_s *)home_sub->get_data(); - } - - void send(const hrt_abstime t) - { - - /* we're sending the GPS home periodically to ensure the - * the GCS does pick it up at one point */ - if (home_sub->is_published()) { - home_sub->update(t); - - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home->lat * 1e7), - (int32_t)(home->lon * 1e7), - (int32_t)(home->alt) * 1000.0f); - } - } -}; - - -class MavlinkStreamServoOutputRaw : public MavlinkStream -{ -public: - MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) - { - sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); - } - - const char *get_name() - { - return _name; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamServoOutputRaw(_n); - } - -private: - MavlinkOrbSubscription *act_sub; - struct actuator_outputs_s *act; - - char _name[20]; - unsigned int _n; - -protected: - void subscribe(Mavlink *mavlink) - { - orb_id_t act_topics[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - act_sub = mavlink->add_orb_subscription(act_topics[_n]); - act = (struct actuator_outputs_s *)act_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (act_sub->update(t)) { - mavlink_msg_servo_output_raw_send(_channel, - act->timestamp / 1000, - _n, - act->output[0], - act->output[1], - act->output[2], - act->output[3], - act->output[4], - act->output[5], - act->output[6], - act->output[7]); - } - } -}; - - -class MavlinkStreamHILControls : public MavlinkStream -{ -public: - const char *get_name() - { - return "HIL_CONTROLS"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamHILControls(); - } - -private: - MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; - - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct position_setpoint_triplet_s *pos_sp_triplet; - - MavlinkOrbSubscription *act_sub; - struct actuator_outputs_s *act; - -protected: - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - status = (struct vehicle_status_s *)status_sub->get_data(); - - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); - - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); - act = (struct actuator_outputs_s *)act_sub->get_data(); - } - - void send(const hrt_abstime t) - { - bool updated = act_sub->update(t); - (void)pos_sp_triplet_sub->update(t); - (void)status_sub->update(t); - - if (updated && (status->arming_state == ARMING_STATE_ARMED)) { - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state; - uint8_t mavlink_base_mode; - uint32_t mavlink_custom_mode; - get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - if (mavlink_system.type == MAV_TYPE_QUADROTOR || - mavlink_system.type == MAV_TYPE_HEXAROTOR || - mavlink_system.type == MAV_TYPE_OCTOROTOR) { - /* set number of valid outputs depending on vehicle type */ - unsigned n; - - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; - - case MAV_TYPE_HEXAROTOR: - n = 6; - break; - - default: - n = 8; - break; - } - - /* scale / assign outputs depending on system type */ - float out[8]; - - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ - out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - - } else { - /* send 0 when disarmed */ - out[i] = 0.0f; - } - - } else { - out[i] = -1.0f; - } - } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); - } else { - - /* fixed wing: scale all channels except throttle -1 .. 1 - * because we know that we set the mixers up this way - */ - - float out[8]; - - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; - - for (unsigned i = 0; i < 8; i++) { - if (i != 3) { - /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ - out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); - - } else { - - /* scale fake PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - } - - } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); - } - } - } -}; - - -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream -{ -public: - const char *get_name() - { - return "GLOBAL_POSITION_SETPOINT_INT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamGlobalPositionSetpointInt(); - } - -private: - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct position_setpoint_triplet_s *pos_sp_triplet; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } void send(const hrt_abstime t) { - if (pos_sp_triplet_sub->update(t)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet->current.lat * 1e7), - (int32_t)(pos_sp_triplet->current.lon * 1e7), - (int32_t)(pos_sp_triplet->current.alt * 1000), - (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); - } - } -}; - - -class MavlinkStreamLocalPositionSetpoint : public MavlinkStream -{ -public: - const char *get_name() - { - return "LOCAL_POSITION_SETPOINT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamLocalPositionSetpoint(); - } - -private: - MavlinkOrbSubscription *pos_sp_sub; - struct vehicle_local_position_setpoint_s *pos_sp; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); - pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sp_sub->update(t)) { - mavlink_msg_local_position_setpoint_send(_channel, - MAV_FRAME_LOCAL_NED, - pos_sp->x, - pos_sp->y, - pos_sp->z, - pos_sp->yaw); - } - } -}; - - -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() - { - return "ROLL_PITCH_YAW_THRUST_SETPOINT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawThrustSetpoint(); - } - -private: - MavlinkOrbSubscription *att_sp_sub; - struct vehicle_attitude_setpoint_s *att_sp; - -protected: - void subscribe(Mavlink *mavlink) - { - att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); - att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (att_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, - att_sp->timestamp / 1000, - att_sp->roll_body, - att_sp->pitch_body, - att_sp->yaw_body, - att_sp->thrust); - } - } -}; - - -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() - { - return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); - } - -private: - MavlinkOrbSubscription *att_rates_sp_sub; - struct vehicle_rates_setpoint_s *att_rates_sp; - -protected: - void subscribe(Mavlink *mavlink) - { - att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); - att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (att_rates_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, - att_rates_sp->timestamp / 1000, - att_rates_sp->roll, - att_rates_sp->pitch, - att_rates_sp->yaw, - att_rates_sp->thrust); - } - } -}; - - -class MavlinkStreamRCChannelsRaw : public MavlinkStream -{ -public: - const char *get_name() - { - return "RC_CHANNELS_RAW"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamRCChannelsRaw(); - } - -private: - MavlinkOrbSubscription *rc_sub; - struct rc_input_values *rc; - -protected: - void subscribe(Mavlink *mavlink) - { - rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); - rc = (struct rc_input_values *)rc_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (rc_sub->update(t)) { - const unsigned port_width = 8; - - for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_channel, - rc->timestamp_publication / 1000, - i, - (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, - rc->rssi); - } - } - } -}; - - -class MavlinkStreamManualControl : public MavlinkStream -{ -public: - const char *get_name() - { - return "MANUAL_CONTROL"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamManualControl(); - } - -private: - MavlinkOrbSubscription *manual_sub; - struct manual_control_setpoint_s *manual; - -protected: - void subscribe(Mavlink *mavlink) - { - manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); - manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (manual_sub->update(t)) { - mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual->x * 1000, - manual->y * 1000, - manual->z * 1000, - manual->r * 1000, - 0); - } - } -}; + struct vehicle_gps_position_s gps; - -class MavlinkStreamOpticalFlow : public MavlinkStream -{ -public: - const char *get_name() - { - return "OPTICAL_FLOW"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamOpticalFlow(); - } - -private: - MavlinkOrbSubscription *flow_sub; - struct optical_flow_s *flow; - -protected: - void subscribe(Mavlink *mavlink) - { - flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); - flow = (struct optical_flow_s *)flow_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (flow_sub->update(t)) { - mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); - } - } -}; - -class MavlinkStreamAttitudeControls : public MavlinkStream -{ -public: - const char *get_name() - { - return "ATTITUDE_CONTROLS"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamAttitudeControls(); - } - -private: - MavlinkOrbSubscription *att_ctrl_sub; - struct actuator_controls_s *att_ctrl; - -protected: - void subscribe(Mavlink *mavlink) - { - att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (att_ctrl_sub->update(t)) { - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "rll ctrl ", - att_ctrl->control[0]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "ptch ctrl ", - att_ctrl->control[1]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "yaw ctrl ", - att_ctrl->control[2]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "thr ctrl ", - att_ctrl->control[3]); - } - } -}; - -class MavlinkStreamNamedValueFloat : public MavlinkStream -{ -public: - const char *get_name() - { - return "NAMED_VALUE_FLOAT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamNamedValueFloat(); - } - -private: - MavlinkOrbSubscription *debug_sub; - struct debug_key_value_s *debug; - -protected: - void subscribe(Mavlink *mavlink) - { - debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); - debug = (struct debug_key_value_s *)debug_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (debug_sub->update(t)) { - /* enforce null termination */ - debug->key[sizeof(debug->key) - 1] = '\0'; - - mavlink_msg_named_value_float_send(_channel, - debug->timestamp_ms, - debug->key, - debug->value); - } - } -}; - -class MavlinkStreamCameraCapture : public MavlinkStream -{ -public: - const char *get_name() - { - return "CAMERA_CAPTURE"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamCameraCapture(); - } - -private: - MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; - -protected: - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - status = (struct vehicle_status_s *)status_sub->get_data(); - } - - void send(const hrt_abstime t) - { - (void)status_sub->update(t); - - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - - /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); - - } else { - /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + if (gps_sub->update(t, &gps)) { + mavlink_msg_gps_raw_int_send(_channel, + gps.timestamp_position, + gps.fix_type, + gps.lat, + gps.lon, + gps.alt, + cm_uint16_from_m_float(gps.eph_m), + cm_uint16_from_m_float(gps.epv_m), + gps.vel_m_s * 100.0f, + _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps.satellites_visible); } } }; -class MavlinkStreamDistanceSensor : public MavlinkStream -{ -public: - const char *get_name() - { - return "DISTANCE_SENSOR"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamDistanceSensor(); - } - -private: - MavlinkOrbSubscription *range_sub; - struct range_finder_report *range; - -protected: - void subscribe(Mavlink *mavlink) - { - range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); - range = (struct range_finder_report *)range_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (range_sub->update(t)) { - - uint8_t type; - - switch (range->type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; - break; - } - - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; - - mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, - range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); - } - } -}; -MavlinkStream *streams_list[] = { - new MavlinkStreamHeartbeat(), - new MavlinkStreamSysStatus(), - new MavlinkStreamHighresIMU(), - new MavlinkStreamAttitude(), - new MavlinkStreamAttitudeQuaternion(), - new MavlinkStreamVFRHUD(), - new MavlinkStreamGPSRawInt(), - new MavlinkStreamGlobalPositionInt(), - new MavlinkStreamLocalPositionNED(), - new MavlinkStreamGPSGlobalOrigin(), - new MavlinkStreamServoOutputRaw(0), - new MavlinkStreamServoOutputRaw(1), - new MavlinkStreamServoOutputRaw(2), - new MavlinkStreamServoOutputRaw(3), - new MavlinkStreamHILControls(), - new MavlinkStreamGlobalPositionSetpointInt(), - new MavlinkStreamLocalPositionSetpoint(), - new MavlinkStreamRollPitchYawThrustSetpoint(), - new MavlinkStreamRollPitchYawRatesThrustSetpoint(), - new MavlinkStreamRCChannelsRaw(), - new MavlinkStreamManualControl(), - new MavlinkStreamOpticalFlow(), - new MavlinkStreamAttitudeControls(), - new MavlinkStreamNamedValueFloat(), - new MavlinkStreamCameraCapture(), - new MavlinkStreamDistanceSensor(), - new MavlinkStreamViconPositionEstimate(), +// class MavlinkStreamGlobalPositionInt : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "GLOBAL_POSITION_INT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamGlobalPositionInt(); +// } + +// private: +// MavlinkOrbSubscription *pos_sub; +// struct vehicle_global_position_s *pos; + +// MavlinkOrbSubscription *home_sub; +// struct home_position_s *home; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); +// pos = (struct vehicle_global_position_s *)pos_sub->get_data(); + +// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// home = (struct home_position_s *)home_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// bool updated = pos_sub->update(t); +// updated |= home_sub->update(t); + +// if (updated) { +// mavlink_msg_global_position_int_send(_channel, +// pos->timestamp / 1000, +// pos->lat * 1e7, +// pos->lon * 1e7, +// pos->alt * 1000.0f, +// (pos->alt - home->alt) * 1000.0f, +// pos->vel_n * 100.0f, +// pos->vel_e * 100.0f, +// pos->vel_d * 100.0f, +// _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); +// } +// } +// }; + + +// class MavlinkStreamLocalPositionNED : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "LOCAL_POSITION_NED"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamLocalPositionNED(); +// } + +// private: +// MavlinkOrbSubscription *pos_sub; +// struct vehicle_local_position_s *pos; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); +// pos = (struct vehicle_local_position_s *)pos_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (pos_sub->update(t)) { +// mavlink_msg_local_position_ned_send(_channel, +// pos->timestamp / 1000, +// pos->x, +// pos->y, +// pos->z, +// pos->vx, +// pos->vy, +// pos->vz); +// } +// } +// }; + + + +// class MavlinkStreamViconPositionEstimate : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "VICON_POSITION_ESTIMATE"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamViconPositionEstimate(); +// } + +// private: +// MavlinkOrbSubscription *pos_sub; +// struct vehicle_vicon_position_s *pos; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); +// pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (pos_sub->update(t)) { +// mavlink_msg_vicon_position_estimate_send(_channel, +// pos->timestamp / 1000, +// pos->x, +// pos->y, +// pos->z, +// pos->roll, +// pos->pitch, +// pos->yaw); +// } +// } +// }; + + +// class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "GPS_GLOBAL_ORIGIN"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamGPSGlobalOrigin(); +// } + +// private: +// MavlinkOrbSubscription *home_sub; +// struct home_position_s *home; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// home = (struct home_position_s *)home_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { + +// /* we're sending the GPS home periodically to ensure the +// * the GCS does pick it up at one point */ +// if (home_sub->is_published()) { +// home_sub->update(t); + +// mavlink_msg_gps_global_origin_send(_channel, +// (int32_t)(home->lat * 1e7), +// (int32_t)(home->lon * 1e7), +// (int32_t)(home->alt) * 1000.0f); +// } +// } +// }; + + +// class MavlinkStreamServoOutputRaw : public MavlinkStream +// { +// public: +// MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) +// { +// sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); +// } + +// const char *get_name() +// { +// return _name; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamServoOutputRaw(_n); +// } + +// private: +// MavlinkOrbSubscription *act_sub; +// struct actuator_outputs_s *act; + +// char _name[20]; +// unsigned int _n; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// orb_id_t act_topics[] = { +// ORB_ID(actuator_outputs_0), +// ORB_ID(actuator_outputs_1), +// ORB_ID(actuator_outputs_2), +// ORB_ID(actuator_outputs_3) +// }; + +// act_sub = mavlink->add_orb_subscription(act_topics[_n]); +// act = (struct actuator_outputs_s *)act_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (act_sub->update(t)) { +// mavlink_msg_servo_output_raw_send(_channel, +// act->timestamp / 1000, +// _n, +// act->output[0], +// act->output[1], +// act->output[2], +// act->output[3], +// act->output[4], +// act->output[5], +// act->output[6], +// act->output[7]); +// } +// } +// }; + + +// class MavlinkStreamHILControls : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "HIL_CONTROLS"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamHILControls(); +// } + +// private: +// MavlinkOrbSubscription *status_sub; +// struct vehicle_status_s *status; + +// MavlinkOrbSubscription *pos_sp_triplet_sub; +// struct position_setpoint_triplet_s *pos_sp_triplet; + +// MavlinkOrbSubscription *act_sub; +// struct actuator_outputs_s *act; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// status = (struct vehicle_status_s *)status_sub->get_data(); + +// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); + +// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); +// act = (struct actuator_outputs_s *)act_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// bool updated = act_sub->update(t); +// (void)pos_sp_triplet_sub->update(t); +// (void)status_sub->update(t); + +// if (updated && (status->arming_state == ARMING_STATE_ARMED)) { +// /* translate the current syste state to mavlink state and mode */ +// uint8_t mavlink_state; +// uint8_t mavlink_base_mode; +// uint32_t mavlink_custom_mode; +// get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + +// if (mavlink_system.type == MAV_TYPE_QUADROTOR || +// mavlink_system.type == MAV_TYPE_HEXAROTOR || +// mavlink_system.type == MAV_TYPE_OCTOROTOR) { +// /* set number of valid outputs depending on vehicle type */ +// unsigned n; + +// switch (mavlink_system.type) { +// case MAV_TYPE_QUADROTOR: +// n = 4; +// break; + +// case MAV_TYPE_HEXAROTOR: +// n = 6; +// break; + +// default: +// n = 8; +// break; +// } + +// /* scale / assign outputs depending on system type */ +// float out[8]; + +// for (unsigned i = 0; i < 8; i++) { +// if (i < n) { +// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { +// /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ +// out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + +// } else { +// /* send 0 when disarmed */ +// out[i] = 0.0f; +// } + +// } else { +// out[i] = -1.0f; +// } +// } + +// mavlink_msg_hil_controls_send(_channel, +// hrt_absolute_time(), +// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], +// mavlink_base_mode, +// 0); +// } else { + +// /* fixed wing: scale all channels except throttle -1 .. 1 +// * because we know that we set the mixers up this way +// */ + +// float out[8]; + +// const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + +// for (unsigned i = 0; i < 8; i++) { +// if (i != 3) { +// /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ +// out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + +// } else { + +// /* scale fake PWM out 900..2100 us to 0..1 for throttle */ +// out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); +// } + +// } + +// mavlink_msg_hil_controls_send(_channel, +// hrt_absolute_time(), +// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], +// mavlink_base_mode, +// 0); +// } +// } +// } +// }; + + +// class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "GLOBAL_POSITION_SETPOINT_INT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamGlobalPositionSetpointInt(); +// } + +// private: +// MavlinkOrbSubscription *pos_sp_triplet_sub; +// struct position_setpoint_triplet_s *pos_sp_triplet; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (pos_sp_triplet_sub->update(t)) { +// mavlink_msg_global_position_setpoint_int_send(_channel, +// MAV_FRAME_GLOBAL, +// (int32_t)(pos_sp_triplet->current.lat * 1e7), +// (int32_t)(pos_sp_triplet->current.lon * 1e7), +// (int32_t)(pos_sp_triplet->current.alt * 1000), +// (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); +// } +// } +// }; + + +// class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "LOCAL_POSITION_SETPOINT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamLocalPositionSetpoint(); +// } + +// private: +// MavlinkOrbSubscription *pos_sp_sub; +// struct vehicle_local_position_setpoint_s *pos_sp; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); +// pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (pos_sp_sub->update(t)) { +// mavlink_msg_local_position_setpoint_send(_channel, +// MAV_FRAME_LOCAL_NED, +// pos_sp->x, +// pos_sp->y, +// pos_sp->z, +// pos_sp->yaw); +// } +// } +// }; + + +// class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "ROLL_PITCH_YAW_THRUST_SETPOINT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamRollPitchYawThrustSetpoint(); +// } + +// private: +// MavlinkOrbSubscription *att_sp_sub; +// struct vehicle_attitude_setpoint_s *att_sp; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); +// att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (att_sp_sub->update(t)) { +// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, +// att_sp->timestamp / 1000, +// att_sp->roll_body, +// att_sp->pitch_body, +// att_sp->yaw_body, +// att_sp->thrust); +// } +// } +// }; + + +// class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); +// } + +// private: +// MavlinkOrbSubscription *att_rates_sp_sub; +// struct vehicle_rates_setpoint_s *att_rates_sp; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); +// att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (att_rates_sp_sub->update(t)) { +// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, +// att_rates_sp->timestamp / 1000, +// att_rates_sp->roll, +// att_rates_sp->pitch, +// att_rates_sp->yaw, +// att_rates_sp->thrust); +// } +// } +// }; + + +// class MavlinkStreamRCChannelsRaw : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "RC_CHANNELS_RAW"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamRCChannelsRaw(); +// } + +// private: +// MavlinkOrbSubscription *rc_sub; +// struct rc_input_values *rc; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); +// rc = (struct rc_input_values *)rc_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (rc_sub->update(t)) { +// const unsigned port_width = 8; + +// for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { +// /* Channels are sent in MAVLink main loop at a fixed interval */ +// mavlink_msg_rc_channels_raw_send(_channel, +// rc->timestamp_publication / 1000, +// i, +// (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, +// rc->rssi); +// } +// } +// } +// }; + + +// class MavlinkStreamManualControl : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "MANUAL_CONTROL"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamManualControl(); +// } + +// private: +// MavlinkOrbSubscription *manual_sub; +// struct manual_control_setpoint_s *manual; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); +// manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (manual_sub->update(t)) { +// mavlink_msg_manual_control_send(_channel, +// mavlink_system.sysid, +// manual->x * 1000, +// manual->y * 1000, +// manual->z * 1000, +// manual->r * 1000, +// 0); +// } +// } +// }; + + +// class MavlinkStreamOpticalFlow : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "OPTICAL_FLOW"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamOpticalFlow(); +// } + +// private: +// MavlinkOrbSubscription *flow_sub; +// struct optical_flow_s *flow; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); +// flow = (struct optical_flow_s *)flow_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (flow_sub->update(t)) { +// mavlink_msg_optical_flow_send(_channel, +// flow->timestamp, +// flow->sensor_id, +// flow->flow_raw_x, flow->flow_raw_y, +// flow->flow_comp_x_m, flow->flow_comp_y_m, +// flow->quality, +// flow->ground_distance_m); +// } +// } +// }; + +// class MavlinkStreamAttitudeControls : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "ATTITUDE_CONTROLS"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamAttitudeControls(); +// } + +// private: +// MavlinkOrbSubscription *att_ctrl_sub; +// struct actuator_controls_s *att_ctrl; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); +// att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (att_ctrl_sub->update(t)) { +// /* send, add spaces so that string buffer is at least 10 chars long */ +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl->timestamp / 1000, +// "rll ctrl ", +// att_ctrl->control[0]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl->timestamp / 1000, +// "ptch ctrl ", +// att_ctrl->control[1]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl->timestamp / 1000, +// "yaw ctrl ", +// att_ctrl->control[2]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl->timestamp / 1000, +// "thr ctrl ", +// att_ctrl->control[3]); +// } +// } +// }; + +// class MavlinkStreamNamedValueFloat : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "NAMED_VALUE_FLOAT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamNamedValueFloat(); +// } + +// private: +// MavlinkOrbSubscription *debug_sub; +// struct debug_key_value_s *debug; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); +// debug = (struct debug_key_value_s *)debug_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (debug_sub->update(t)) { +// /* enforce null termination */ +// debug->key[sizeof(debug->key) - 1] = '\0'; + +// mavlink_msg_named_value_float_send(_channel, +// debug->timestamp_ms, +// debug->key, +// debug->value); +// } +// } +// }; + +// class MavlinkStreamCameraCapture : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "CAMERA_CAPTURE"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamCameraCapture(); +// } + +// private: +// MavlinkOrbSubscription *status_sub; +// struct vehicle_status_s *status; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// status = (struct vehicle_status_s *)status_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// (void)status_sub->update(t); + +// if (status->arming_state == ARMING_STATE_ARMED +// || status->arming_state == ARMING_STATE_ARMED_ERROR) { + +// /* send camera capture on */ +// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + +// } else { +// /* send camera capture off */ +// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); +// } +// } +// }; + +// class MavlinkStreamDistanceSensor : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "DISTANCE_SENSOR"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamDistanceSensor(); +// } + +// private: +// MavlinkOrbSubscription *range_sub; +// struct range_finder_report *range; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); +// range = (struct range_finder_report *)range_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (range_sub->update(t)) { + +// uint8_t type; + +// switch (range->type) { +// case RANGE_FINDER_TYPE_LASER: +// type = MAV_DISTANCE_SENSOR_LASER; +// break; +// } + +// uint8_t id = 0; +// uint8_t orientation = 0; +// uint8_t covariance = 20; + +// mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, +// range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); +// } +// } +// }; + +StreamListItem *streams_list[] = { + new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), + new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), + new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), + new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), + new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), + new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), + new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), + // new MavlinkStreamGlobalPositionInt(), + // new MavlinkStreamLocalPositionNED(), + // new MavlinkStreamGPSGlobalOrigin(), + // new MavlinkStreamServoOutputRaw(0), + // new MavlinkStreamServoOutputRaw(1), + // new MavlinkStreamServoOutputRaw(2), + // new MavlinkStreamServoOutputRaw(3), + // new MavlinkStreamHILControls(), + // new MavlinkStreamGlobalPositionSetpointInt(), + // new MavlinkStreamLocalPositionSetpoint(), + // new MavlinkStreamRollPitchYawThrustSetpoint(), + // new MavlinkStreamRollPitchYawRatesThrustSetpoint(), + // new MavlinkStreamRCChannelsRaw(), + // new MavlinkStreamManualControl(), + // new MavlinkStreamOpticalFlow(), + // new MavlinkStreamAttitudeControls(), + // new MavlinkStreamNamedValueFloat(), + // new MavlinkStreamCameraCapture(), + // new MavlinkStreamDistanceSensor(), + // new MavlinkStreamViconPositionEstimate(), nullptr }; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index b8823263a..ee64d0e42 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -43,6 +43,19 @@ #include "mavlink_stream.h" -extern MavlinkStream *streams_list[]; +class StreamListItem { + +public: + MavlinkStream* (*new_instance)(); + const char* (*get_name)(); + + StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) : + new_instance(inst), + get_name(name) {}; + + ~StreamListItem() {}; +}; + +extern StreamListItem *streams_list[]; #endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index d432edd2b..0a23fb01e 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -53,30 +53,21 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _last_check(0), next(nullptr) { - _data = malloc(topic->o_size); - memset(_data, 0, topic->o_size); } MavlinkOrbSubscription::~MavlinkOrbSubscription() { close(_fd); - free(_data); } -const orb_id_t -MavlinkOrbSubscription::get_topic() +orb_id_t +MavlinkOrbSubscription::get_topic() const { return _topic; } -void * -MavlinkOrbSubscription::get_data() -{ - return _data; -} - bool -MavlinkOrbSubscription::update(const hrt_abstime t) +MavlinkOrbSubscription::update(const hrt_abstime t, void* data) { if (_last_check == t) { /* already checked right now, return result of the check */ @@ -86,8 +77,8 @@ MavlinkOrbSubscription::update(const hrt_abstime t) _last_check = t; orb_check(_fd, &_updated); - if (_updated) { - orb_copy(_topic, _fd, _data); + if (_updated && data) { + orb_copy(_topic, _fd, data); _published = true; return true; } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 5c6543e81..abd4031bd 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -48,12 +48,12 @@ class MavlinkOrbSubscription { public: - MavlinkOrbSubscription *next; /*< pointer to next subscription in list */ + MavlinkOrbSubscription *next; ///< pointer to next subscription in list MavlinkOrbSubscription(const orb_id_t topic); ~MavlinkOrbSubscription(); - bool update(const hrt_abstime t); + bool update(const hrt_abstime t, void* data); /** * Check if the topic has been published. @@ -62,16 +62,14 @@ public: * @return true if the topic has been published at least once. */ bool is_published(); - void *get_data(); - const orb_id_t get_topic(); + orb_id_t get_topic() const; private: - const orb_id_t _topic; /*< topic metadata */ - int _fd; /*< subscription handle */ - bool _published; /*< topic was ever published */ - void *_data; /*< pointer to data buffer */ - hrt_abstime _last_check; /*< time of last check */ - bool _updated; /*< updated on last check */ + const orb_id_t _topic; ///< topic metadata + int _fd; ///< subscription handle + bool _published; ///< topic was ever published + hrt_abstime _last_check; ///< time of last check + bool _updated; ///< updated on last check }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 72b9ee83a..666b3a8cd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -950,7 +950,6 @@ MavlinkReceiver::receive_start(Mavlink *parent) (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2900); - pthread_t thread; pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index def40d9ad..eb881edd7 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -50,14 +50,6 @@ class MavlinkStream; class MavlinkStream { -private: - hrt_abstime _last_sent; - -protected: - mavlink_channel_t _channel; - unsigned int _interval; - - virtual void send(const hrt_abstime t) = 0; public: MavlinkStream *next; @@ -67,9 +59,19 @@ public: void set_interval(const unsigned int interval); void set_channel(mavlink_channel_t channel); int update(const hrt_abstime t); - virtual MavlinkStream *new_instance() = 0; + static MavlinkStream *new_instance(); + static const char *get_name_static(); virtual void subscribe(Mavlink *mavlink) = 0; - virtual const char *get_name() = 0; + virtual const char *get_name() const = 0; + +protected: + mavlink_channel_t _channel; + unsigned int _interval; + + virtual void send(const hrt_abstime t) = 0; + +private: + hrt_abstime _last_sent; }; -- cgit v1.2.3 From 7926a1f8aed851d8ac22538279c11660dc645f20 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 18:22:03 +0200 Subject: Converted streams to new API, saving a bunch of RAM --- src/modules/mavlink/mavlink_messages.cpp | 382 +++++++++++++++++-------------- 1 file changed, 212 insertions(+), 170 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4bb827116..3a82f1f05 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -596,55 +596,57 @@ protected: }; -// class MavlinkStreamGlobalPositionInt : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "GLOBAL_POSITION_INT"; -// } +class MavlinkStreamGlobalPositionInt : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGlobalPositionInt::get_name_static(); + } -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamGlobalPositionInt(); -// } + static const char *get_name_static() + { + return "GLOBAL_POSITION_INT"; + } -// private: -// MavlinkOrbSubscription *pos_sub; -// struct vehicle_global_position_s *pos; + static MavlinkStream *new_instance() + { + return new MavlinkStreamGlobalPositionInt(); + } -// MavlinkOrbSubscription *home_sub; -// struct home_position_s *home; +private: + MavlinkOrbSubscription *pos_sub; + MavlinkOrbSubscription *home_sub; -// protected: -// void subscribe(Mavlink *mavlink) -// { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); -// pos = (struct vehicle_global_position_s *)pos_sub->get_data(); +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); + } -// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); -// home = (struct home_position_s *)home_sub->get_data(); -// } + void send(const hrt_abstime t) + { + struct vehicle_global_position_s pos; + struct home_position_s home; -// void send(const hrt_abstime t) -// { -// bool updated = pos_sub->update(t); -// updated |= home_sub->update(t); - -// if (updated) { -// mavlink_msg_global_position_int_send(_channel, -// pos->timestamp / 1000, -// pos->lat * 1e7, -// pos->lon * 1e7, -// pos->alt * 1000.0f, -// (pos->alt - home->alt) * 1000.0f, -// pos->vel_n * 100.0f, -// pos->vel_e * 100.0f, -// pos->vel_d * 100.0f, -// _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); -// } -// } -// }; + bool updated = pos_sub->update(t, &pos); + updated |= home_sub->update(t, &home); + + if (updated) { + mavlink_msg_global_position_int_send(_channel, + pos.timestamp / 1000, + pos.lat * 1e7, + pos.lon * 1e7, + pos.alt * 1000.0f, + (pos.alt - home.alt) * 1000.0f, + pos.vel_n * 100.0f, + pos.vel_e * 100.0f, + pos.vel_d * 100.0f, + _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); + } + } +}; // class MavlinkStreamLocalPositionNED : public MavlinkStream @@ -675,13 +677,13 @@ protected: // { // if (pos_sub->update(t)) { // mavlink_msg_local_position_ned_send(_channel, -// pos->timestamp / 1000, -// pos->x, -// pos->y, -// pos->z, -// pos->vx, -// pos->vy, -// pos->vz); +// pos.timestamp / 1000, +// pos.x, +// pos.y, +// pos.z, +// pos.vx, +// pos.vy, +// pos.vz); // } // } // }; @@ -716,13 +718,13 @@ protected: // { // if (pos_sub->update(t)) { // mavlink_msg_vicon_position_estimate_send(_channel, -// pos->timestamp / 1000, -// pos->x, -// pos->y, -// pos->z, -// pos->roll, -// pos->pitch, -// pos->yaw); +// pos.timestamp / 1000, +// pos.x, +// pos.y, +// pos.z, +// pos.roll, +// pos.pitch, +// pos.yaw); // } // } // }; @@ -1106,131 +1108,171 @@ protected: // }; -// class MavlinkStreamRCChannelsRaw : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "RC_CHANNELS_RAW"; -// } +class MavlinkStreamRCChannelsRaw : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRCChannelsRaw::get_name_static(); + } -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamRCChannelsRaw(); -// } + static const char *get_name_static() + { + return "RC_CHANNELS_RAW"; + } -// private: -// MavlinkOrbSubscription *rc_sub; -// struct rc_input_values *rc; + static MavlinkStream *new_instance() + { + return new MavlinkStreamRCChannelsRaw(); + } -// protected: -// void subscribe(Mavlink *mavlink) -// { -// rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); -// rc = (struct rc_input_values *)rc_sub->get_data(); -// } +private: + MavlinkOrbSubscription *rc_sub; -// void send(const hrt_abstime t) -// { -// if (rc_sub->update(t)) { -// const unsigned port_width = 8; - -// for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { -// /* Channels are sent in MAVLink main loop at a fixed interval */ -// mavlink_msg_rc_channels_raw_send(_channel, -// rc->timestamp_publication / 1000, -// i, -// (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, -// rc->rssi); -// } -// } -// } -// }; +protected: + void subscribe(Mavlink *mavlink) + { + rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); + } + + void send(const hrt_abstime t) + { + struct rc_input_values rc; + + if (rc_sub->update(t, &rc)) { + const unsigned port_width = 8; + + // Deprecated message (but still needed for compatibility!) + for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(_channel, + rc.timestamp_publication / 1000, + i, + (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, + rc.rssi); + } + // New message + mavlink_msg_rc_channels_send(_channel, + rc.timestamp_publication / 1000, + ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), + ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), + ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), + ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), + ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), + ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), + ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), + ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), + ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), + ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), + ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), + ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), + ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), + ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), + ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), + ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), + ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), + ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), + rc.channel_count, + rc.rssi); + } + } +}; -// class MavlinkStreamManualControl : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "MANUAL_CONTROL"; -// } -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamManualControl(); -// } +class MavlinkStreamManualControl : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamManualControl::get_name_static(); + } -// private: -// MavlinkOrbSubscription *manual_sub; -// struct manual_control_setpoint_s *manual; + static const char *get_name_static() + { + return "MANUAL_CONTROL"; + } -// protected: -// void subscribe(Mavlink *mavlink) -// { -// manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); -// manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); -// } + static MavlinkStream *new_instance() + { + return new MavlinkStreamManualControl(); + } -// void send(const hrt_abstime t) -// { -// if (manual_sub->update(t)) { -// mavlink_msg_manual_control_send(_channel, -// mavlink_system.sysid, -// manual->x * 1000, -// manual->y * 1000, -// manual->z * 1000, -// manual->r * 1000, -// 0); -// } -// } -// }; +private: + MavlinkOrbSubscription *manual_sub; +protected: + void subscribe(Mavlink *mavlink) + { + manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); + } -// class MavlinkStreamOpticalFlow : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "OPTICAL_FLOW"; -// } + void send(const hrt_abstime t) + { + struct manual_control_setpoint_s manual; + + if (manual_sub->update(t, &manual)) { + mavlink_msg_manual_control_send(_channel, + mavlink_system.sysid, + manual.x * 1000, + manual.y * 1000, + manual.z * 1000, + manual.r * 1000, + 0); + } + } +}; -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamOpticalFlow(); -// } -// private: -// MavlinkOrbSubscription *flow_sub; -// struct optical_flow_s *flow; +class MavlinkStreamOpticalFlow : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamOpticalFlow::get_name_static(); + } -// protected: -// void subscribe(Mavlink *mavlink) -// { -// flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); -// flow = (struct optical_flow_s *)flow_sub->get_data(); -// } + static const char *get_name_static() + { + return "OPTICAL_FLOW"; + } -// void send(const hrt_abstime t) -// { -// if (flow_sub->update(t)) { -// mavlink_msg_optical_flow_send(_channel, -// flow->timestamp, -// flow->sensor_id, -// flow->flow_raw_x, flow->flow_raw_y, -// flow->flow_comp_x_m, flow->flow_comp_y_m, -// flow->quality, -// flow->ground_distance_m); -// } -// } -// }; + static MavlinkStream *new_instance() + { + return new MavlinkStreamOpticalFlow(); + } + +private: + MavlinkOrbSubscription *flow_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); + } + + void send(const hrt_abstime t) + { + struct optical_flow_s flow; + + if (flow_sub->update(t, &flow)) { + mavlink_msg_optical_flow_send(_channel, + flow.timestamp, + flow.sensor_id, + flow.flow_raw_x, flow.flow_raw_y, + flow.flow_comp_x_m, flow.flow_comp_y_m, + flow.quality, + flow.ground_distance_m); + } + } +}; // class MavlinkStreamAttitudeControls : public MavlinkStream // { @@ -1413,7 +1455,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), - // new MavlinkStreamGlobalPositionInt(), + new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), // new MavlinkStreamLocalPositionNED(), // new MavlinkStreamGPSGlobalOrigin(), // new MavlinkStreamServoOutputRaw(0), @@ -1425,9 +1467,9 @@ StreamListItem *streams_list[] = { // new MavlinkStreamLocalPositionSetpoint(), // new MavlinkStreamRollPitchYawThrustSetpoint(), // new MavlinkStreamRollPitchYawRatesThrustSetpoint(), - // new MavlinkStreamRCChannelsRaw(), - // new MavlinkStreamManualControl(), - // new MavlinkStreamOpticalFlow(), + new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), + new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), + new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), // new MavlinkStreamAttitudeControls(), // new MavlinkStreamNamedValueFloat(), // new MavlinkStreamCameraCapture(), -- cgit v1.2.3 From 9b2370e3873d65b65fd2dc2257beed50df6ac892 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 17:10:11 +0200 Subject: mavlink app: Fix compile error --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 340b20e1b..e9932aac3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1483,9 +1483,9 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) void Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) { - uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); mavlink_send_uart_bytes(_channel, buf, len); } -- cgit v1.2.3 From 7bfcaafc1631cab603191777e2e63f69755e334d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 17:10:33 +0200 Subject: mavlink app: Finish porting all messages to new scheme --- src/modules/mavlink/mavlink_messages.cpp | 1362 ++++++++++++++++-------------- 1 file changed, 717 insertions(+), 645 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 3a82f1f05..748c1f69a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -649,463 +649,514 @@ protected: }; -// class MavlinkStreamLocalPositionNED : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "LOCAL_POSITION_NED"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamLocalPositionNED(); -// } - -// private: -// MavlinkOrbSubscription *pos_sub; -// struct vehicle_local_position_s *pos; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); -// pos = (struct vehicle_local_position_s *)pos_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (pos_sub->update(t)) { -// mavlink_msg_local_position_ned_send(_channel, -// pos.timestamp / 1000, -// pos.x, -// pos.y, -// pos.z, -// pos.vx, -// pos.vy, -// pos.vz); -// } -// } -// }; - - - -// class MavlinkStreamViconPositionEstimate : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "VICON_POSITION_ESTIMATE"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamViconPositionEstimate(); -// } - -// private: -// MavlinkOrbSubscription *pos_sub; -// struct vehicle_vicon_position_s *pos; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); -// pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (pos_sub->update(t)) { -// mavlink_msg_vicon_position_estimate_send(_channel, -// pos.timestamp / 1000, -// pos.x, -// pos.y, -// pos.z, -// pos.roll, -// pos.pitch, -// pos.yaw); -// } -// } -// }; - - -// class MavlinkStreamGPSGlobalOrigin : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "GPS_GLOBAL_ORIGIN"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamGPSGlobalOrigin(); -// } - -// private: -// MavlinkOrbSubscription *home_sub; -// struct home_position_s *home; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); -// home = (struct home_position_s *)home_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { - -// /* we're sending the GPS home periodically to ensure the -// * the GCS does pick it up at one point */ -// if (home_sub->is_published()) { -// home_sub->update(t); - -// mavlink_msg_gps_global_origin_send(_channel, -// (int32_t)(home->lat * 1e7), -// (int32_t)(home->lon * 1e7), -// (int32_t)(home->alt) * 1000.0f); -// } -// } -// }; - - -// class MavlinkStreamServoOutputRaw : public MavlinkStream -// { -// public: -// MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) -// { -// sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); -// } - -// const char *get_name() -// { -// return _name; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamServoOutputRaw(_n); -// } - -// private: -// MavlinkOrbSubscription *act_sub; -// struct actuator_outputs_s *act; - -// char _name[20]; -// unsigned int _n; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// orb_id_t act_topics[] = { -// ORB_ID(actuator_outputs_0), -// ORB_ID(actuator_outputs_1), -// ORB_ID(actuator_outputs_2), -// ORB_ID(actuator_outputs_3) -// }; - -// act_sub = mavlink->add_orb_subscription(act_topics[_n]); -// act = (struct actuator_outputs_s *)act_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (act_sub->update(t)) { -// mavlink_msg_servo_output_raw_send(_channel, -// act->timestamp / 1000, -// _n, -// act->output[0], -// act->output[1], -// act->output[2], -// act->output[3], -// act->output[4], -// act->output[5], -// act->output[6], -// act->output[7]); -// } -// } -// }; - - -// class MavlinkStreamHILControls : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "HIL_CONTROLS"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamHILControls(); -// } - -// private: -// MavlinkOrbSubscription *status_sub; -// struct vehicle_status_s *status; - -// MavlinkOrbSubscription *pos_sp_triplet_sub; -// struct position_setpoint_triplet_s *pos_sp_triplet; - -// MavlinkOrbSubscription *act_sub; -// struct actuator_outputs_s *act; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); -// status = (struct vehicle_status_s *)status_sub->get_data(); - -// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); -// pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); - -// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); -// act = (struct actuator_outputs_s *)act_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// bool updated = act_sub->update(t); -// (void)pos_sp_triplet_sub->update(t); -// (void)status_sub->update(t); - -// if (updated && (status->arming_state == ARMING_STATE_ARMED)) { -// /* translate the current syste state to mavlink state and mode */ -// uint8_t mavlink_state; -// uint8_t mavlink_base_mode; -// uint32_t mavlink_custom_mode; -// get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - -// if (mavlink_system.type == MAV_TYPE_QUADROTOR || -// mavlink_system.type == MAV_TYPE_HEXAROTOR || -// mavlink_system.type == MAV_TYPE_OCTOROTOR) { -// /* set number of valid outputs depending on vehicle type */ -// unsigned n; - -// switch (mavlink_system.type) { -// case MAV_TYPE_QUADROTOR: -// n = 4; -// break; - -// case MAV_TYPE_HEXAROTOR: -// n = 6; -// break; - -// default: -// n = 8; -// break; -// } - -// /* scale / assign outputs depending on system type */ -// float out[8]; - -// for (unsigned i = 0; i < 8; i++) { -// if (i < n) { -// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { -// /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ -// out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - -// } else { -// /* send 0 when disarmed */ -// out[i] = 0.0f; -// } - -// } else { -// out[i] = -1.0f; -// } -// } - -// mavlink_msg_hil_controls_send(_channel, -// hrt_absolute_time(), -// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], -// mavlink_base_mode, -// 0); -// } else { - -// /* fixed wing: scale all channels except throttle -1 .. 1 -// * because we know that we set the mixers up this way -// */ - -// float out[8]; - -// const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; - -// for (unsigned i = 0; i < 8; i++) { -// if (i != 3) { -// /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ -// out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); - -// } else { - -// /* scale fake PWM out 900..2100 us to 0..1 for throttle */ -// out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); -// } - -// } - -// mavlink_msg_hil_controls_send(_channel, -// hrt_absolute_time(), -// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], -// mavlink_base_mode, -// 0); -// } -// } -// } -// }; - - -// class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "GLOBAL_POSITION_SETPOINT_INT"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamGlobalPositionSetpointInt(); -// } - -// private: -// MavlinkOrbSubscription *pos_sp_triplet_sub; -// struct position_setpoint_triplet_s *pos_sp_triplet; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); -// pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (pos_sp_triplet_sub->update(t)) { -// mavlink_msg_global_position_setpoint_int_send(_channel, -// MAV_FRAME_GLOBAL, -// (int32_t)(pos_sp_triplet->current.lat * 1e7), -// (int32_t)(pos_sp_triplet->current.lon * 1e7), -// (int32_t)(pos_sp_triplet->current.alt * 1000), -// (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); -// } -// } -// }; - - -// class MavlinkStreamLocalPositionSetpoint : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "LOCAL_POSITION_SETPOINT"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamLocalPositionSetpoint(); -// } - -// private: -// MavlinkOrbSubscription *pos_sp_sub; -// struct vehicle_local_position_setpoint_s *pos_sp; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); -// pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (pos_sp_sub->update(t)) { -// mavlink_msg_local_position_setpoint_send(_channel, -// MAV_FRAME_LOCAL_NED, -// pos_sp->x, -// pos_sp->y, -// pos_sp->z, -// pos_sp->yaw); -// } -// } -// }; - - -// class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "ROLL_PITCH_YAW_THRUST_SETPOINT"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamRollPitchYawThrustSetpoint(); -// } - -// private: -// MavlinkOrbSubscription *att_sp_sub; -// struct vehicle_attitude_setpoint_s *att_sp; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); -// att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (att_sp_sub->update(t)) { -// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, -// att_sp->timestamp / 1000, -// att_sp->roll_body, -// att_sp->pitch_body, -// att_sp->yaw_body, -// att_sp->thrust); -// } -// } -// }; - - -// class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); -// } - -// private: -// MavlinkOrbSubscription *att_rates_sp_sub; -// struct vehicle_rates_setpoint_s *att_rates_sp; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); -// att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (att_rates_sp_sub->update(t)) { -// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, -// att_rates_sp->timestamp / 1000, -// att_rates_sp->roll, -// att_rates_sp->pitch, -// att_rates_sp->yaw, -// att_rates_sp->thrust); -// } -// } -// }; +class MavlinkStreamLocalPositionNED : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamLocalPositionNED::get_name_static(); + } + + static const char *get_name_static() + { + return "LOCAL_POSITION_NED"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamLocalPositionNED(); + } + +private: + MavlinkOrbSubscription *pos_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); + } + + void send(const hrt_abstime t) + { + struct vehicle_local_position_s pos; + + if (pos_sub->update(t, &pos)) { + mavlink_msg_local_position_ned_send(_channel, + pos.timestamp / 1000, + pos.x, + pos.y, + pos.z, + pos.vx, + pos.vy, + pos.vz); + } + } +}; + + + +class MavlinkStreamViconPositionEstimate : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamViconPositionEstimate::get_name_static(); + } + + static const char *get_name_static() + { + return "VICON_POSITION_ESTIMATE"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } + +private: + MavlinkOrbSubscription *pos_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + } + + void send(const hrt_abstime t) + { + struct vehicle_vicon_position_s pos; + + if (pos_sub->update(t, &pos)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos.timestamp / 1000, + pos.x, + pos.y, + pos.z, + pos.roll, + pos.pitch, + pos.yaw); + } + } +}; + + +class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGPSGlobalOrigin::get_name_static(); + } + + static const char *get_name_static() + { + return "GPS_GLOBAL_ORIGIN"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamGPSGlobalOrigin(); + } + +private: + MavlinkOrbSubscription *home_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); + } + + void send(const hrt_abstime t) + { + /* we're sending the GPS home periodically to ensure the + * the GCS does pick it up at one point */ + if (home_sub->is_published()) { + struct home_position_s home; + home_sub->update(t, &home); + + mavlink_msg_gps_global_origin_send(_channel, + (int32_t)(home.lat * 1e7), + (int32_t)(home.lon * 1e7), + (int32_t)(home.alt) * 1000.0f); + } + } +}; + + +class MavlinkStreamServoOutputRaw : public MavlinkStream +{ +public: + MavlinkStreamServoOutputRaw() : MavlinkStream() + { + _instance = _n++; + } + + const char *get_name() const + { + return get_name_static_int(_instance); + } + + static const char *get_name_static() + { + return get_name_static_int(_n); + } + + static const char *get_name_static_int(unsigned n) + { + switch (n) { + case 0: + return "SERVO_OUTPUT_RAW_0"; + + case 1: + return "SERVO_OUTPUT_RAW_1"; + + case 2: + return "SERVO_OUTPUT_RAW_2"; + + case 3: + return "SERVO_OUTPUT_RAW_3"; + } + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamServoOutputRaw(); + } + + static unsigned _n; + +private: + MavlinkOrbSubscription *_act_sub; + + unsigned _instance; + +protected: + void subscribe(Mavlink *mavlink) + { + orb_id_t act_topics[] = { + ORB_ID(actuator_outputs_0), + ORB_ID(actuator_outputs_1), + ORB_ID(actuator_outputs_2), + ORB_ID(actuator_outputs_3) + }; + + _act_sub = mavlink->add_orb_subscription(act_topics[_instance]); + } + + void send(const hrt_abstime t) + { + struct actuator_outputs_s act; + + if (_act_sub->update(t, &act)) { + mavlink_msg_servo_output_raw_send(_channel, + act.timestamp / 1000, + _instance, + act.output[0], + act.output[1], + act.output[2], + act.output[3], + act.output[4], + act.output[5], + act.output[6], + act.output[7]); + } + } +}; + + +class MavlinkStreamHILControls : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamHILControls::get_name_static(); + } + + static const char *get_name_static() + { + return "HIL_CONTROLS"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamHILControls(); + } + +private: + MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *pos_sp_triplet_sub; + MavlinkOrbSubscription *act_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); + } + + void send(const hrt_abstime t) + { + struct vehicle_status_s status; + struct position_setpoint_triplet_s pos_sp_triplet; + struct actuator_outputs_s act; + + bool updated = act_sub->update(t, &act); + (void)pos_sp_triplet_sub->update(t, &pos_sp_triplet); + (void)status_sub->update(t, &status); + + if (updated && (status.arming_state == ARMING_STATE_ARMED)) { + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state; + uint8_t mavlink_base_mode; + uint32_t mavlink_custom_mode; + get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + if (mavlink_system.type == MAV_TYPE_QUADROTOR || + mavlink_system.type == MAV_TYPE_HEXAROTOR || + mavlink_system.type == MAV_TYPE_OCTOROTOR) { + /* set number of valid outputs depending on vehicle type */ + unsigned n; + + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; + + case MAV_TYPE_HEXAROTOR: + n = 6; + break; + + default: + n = 8; + break; + } + + /* scale / assign outputs depending on system type */ + float out[8]; + + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } + + } else { + out[i] = -1.0f; + } + } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); + } else { + + /* fixed wing: scale all channels except throttle -1 .. 1 + * because we know that we set the mixers up this way + */ + + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + for (unsigned i = 0; i < 8; i++) { + if (i != 3) { + /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + + } else { + + /* scale fake PWM out 900..2100 us to 0..1 for throttle */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + } + + } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); + } + } + } +}; + + +class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); + } + + static const char *get_name_static() + { + return "GLOBAL_POSITION_SETPOINT_INT"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamGlobalPositionSetpointInt(); + } + +private: + MavlinkOrbSubscription *pos_sp_triplet_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); + } + + void send(const hrt_abstime t) + { + struct position_setpoint_triplet_s pos_sp_triplet; + if (pos_sp_triplet_sub->update(t, &pos_sp_triplet)) { + mavlink_msg_global_position_setpoint_int_send(_channel, + MAV_FRAME_GLOBAL, + (int32_t)(pos_sp_triplet.current.lat * 1e7), + (int32_t)(pos_sp_triplet.current.lon * 1e7), + (int32_t)(pos_sp_triplet.current.alt * 1000), + (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); + } + } +}; + + +class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamLocalPositionSetpoint::get_name_static(); + } + + static const char *get_name_static() + { + return "LOCAL_POSITION_SETPOINT"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamLocalPositionSetpoint(); + } + +private: + MavlinkOrbSubscription *pos_sp_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); + } + + void send(const hrt_abstime t) + { + struct vehicle_local_position_setpoint_s pos_sp; + if (pos_sp_sub->update(t, &pos_sp)) { + mavlink_msg_local_position_setpoint_send(_channel, + MAV_FRAME_LOCAL_NED, + pos_sp.x, + pos_sp.y, + pos_sp.z, + pos_sp.yaw); + } + } +}; + + +class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); + } + + static const char *get_name_static() + { + return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamRollPitchYawThrustSetpoint(); + } + +private: + MavlinkOrbSubscription *att_sp_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); + } + + void send(const hrt_abstime t) + { + struct vehicle_attitude_setpoint_s att_sp; + if (att_sp_sub->update(t, &att_sp)) { + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, + att_sp.timestamp / 1000, + att_sp.roll_body, + att_sp.pitch_body, + att_sp.yaw_body, + att_sp.thrust); + } + } +}; + + +class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); + } + + static const char *get_name_static() + { + return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); + } + +private: + MavlinkOrbSubscription *att_rates_sp_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); + } + + void send(const hrt_abstime t) + { + struct vehicle_rates_setpoint_s att_rates_sp; + if (att_rates_sp_sub->update(t, &att_rates_sp)) { + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, + att_rates_sp.timestamp / 1000, + att_rates_sp.roll, + att_rates_sp.pitch, + att_rates_sp.yaw, + att_rates_sp.thrust); + } + } +}; class MavlinkStreamRCChannelsRaw : public MavlinkStream @@ -1274,178 +1325,199 @@ protected: } }; -// class MavlinkStreamAttitudeControls : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "ATTITUDE_CONTROLS"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamAttitudeControls(); -// } - -// private: -// MavlinkOrbSubscription *att_ctrl_sub; -// struct actuator_controls_s *att_ctrl; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); -// att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (att_ctrl_sub->update(t)) { -// /* send, add spaces so that string buffer is at least 10 chars long */ -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl->timestamp / 1000, -// "rll ctrl ", -// att_ctrl->control[0]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl->timestamp / 1000, -// "ptch ctrl ", -// att_ctrl->control[1]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl->timestamp / 1000, -// "yaw ctrl ", -// att_ctrl->control[2]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl->timestamp / 1000, -// "thr ctrl ", -// att_ctrl->control[3]); -// } -// } -// }; - -// class MavlinkStreamNamedValueFloat : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "NAMED_VALUE_FLOAT"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamNamedValueFloat(); -// } - -// private: -// MavlinkOrbSubscription *debug_sub; -// struct debug_key_value_s *debug; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); -// debug = (struct debug_key_value_s *)debug_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (debug_sub->update(t)) { -// /* enforce null termination */ -// debug->key[sizeof(debug->key) - 1] = '\0'; - -// mavlink_msg_named_value_float_send(_channel, -// debug->timestamp_ms, -// debug->key, -// debug->value); -// } -// } -// }; - -// class MavlinkStreamCameraCapture : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "CAMERA_CAPTURE"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamCameraCapture(); -// } - -// private: -// MavlinkOrbSubscription *status_sub; -// struct vehicle_status_s *status; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); -// status = (struct vehicle_status_s *)status_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// (void)status_sub->update(t); - -// if (status->arming_state == ARMING_STATE_ARMED -// || status->arming_state == ARMING_STATE_ARMED_ERROR) { - -// /* send camera capture on */ -// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); - -// } else { -// /* send camera capture off */ -// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); -// } -// } -// }; - -// class MavlinkStreamDistanceSensor : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "DISTANCE_SENSOR"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamDistanceSensor(); -// } - -// private: -// MavlinkOrbSubscription *range_sub; -// struct range_finder_report *range; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); -// range = (struct range_finder_report *)range_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (range_sub->update(t)) { - -// uint8_t type; - -// switch (range->type) { -// case RANGE_FINDER_TYPE_LASER: -// type = MAV_DISTANCE_SENSOR_LASER; -// break; -// } - -// uint8_t id = 0; -// uint8_t orientation = 0; -// uint8_t covariance = 20; - -// mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, -// range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); -// } -// } -// }; +class MavlinkStreamAttitudeControls : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAttitudeControls::get_name_static(); + } + + static const char *get_name_static() + { + return "ATTITUDE_CONTROLS"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamAttitudeControls(); + } + +private: + MavlinkOrbSubscription *att_ctrl_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + } + + void send(const hrt_abstime t) + { + struct actuator_controls_s att_ctrl; + + if (att_ctrl_sub->update(t, &att_ctrl)) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(_channel, + att_ctrl.timestamp / 1000, + "rll ctrl ", + att_ctrl.control[0]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl.timestamp / 1000, + "ptch ctrl ", + att_ctrl.control[1]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl.timestamp / 1000, + "yaw ctrl ", + att_ctrl.control[2]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl.timestamp / 1000, + "thr ctrl ", + att_ctrl.control[3]); + } + } +}; + +class MavlinkStreamNamedValueFloat : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamNamedValueFloat::get_name_static(); + } + + static const char *get_name_static() + { + return "NAMED_VALUE_FLOAT"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamNamedValueFloat(); + } + +private: + MavlinkOrbSubscription *debug_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); + } + + void send(const hrt_abstime t) + { + struct debug_key_value_s debug; + + if (debug_sub->update(t, &debug)) { + /* enforce null termination */ + debug.key[sizeof(debug.key) - 1] = '\0'; + + mavlink_msg_named_value_float_send(_channel, + debug.timestamp_ms, + debug.key, + debug.value); + } + } +}; + +class MavlinkStreamCameraCapture : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamCameraCapture::get_name_static(); + } + + static const char *get_name_static() + { + return "CAMERA_CAPTURE"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamCameraCapture(); + } + +private: + MavlinkOrbSubscription *status_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + } + + void send(const hrt_abstime t) + { + struct vehicle_status_s status; + (void)status_sub->update(t, &status); + + if (status.arming_state == ARMING_STATE_ARMED + || status.arming_state == ARMING_STATE_ARMED_ERROR) { + + /* send camera capture on */ + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + + } else { + /* send camera capture off */ + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + } + } +}; + +class MavlinkStreamDistanceSensor : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamDistanceSensor::get_name_static(); + } + + static const char *get_name_static() + { + return "DISTANCE_SENSOR"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamDistanceSensor(); + } + +private: + MavlinkOrbSubscription *range_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); + } + + void send(const hrt_abstime t) + { + struct range_finder_report range; + + if (range_sub->update(t, &range)) { + + uint8_t type; + + switch (range.type) { + case RANGE_FINDER_TYPE_LASER: + type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + uint8_t id = 0; + uint8_t orientation = 0; + uint8_t covariance = 20; + + mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, + range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); + } + } +}; + +unsigned MavlinkStreamServoOutputRaw::_n = 0; StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), @@ -1456,24 +1528,24 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), - // new MavlinkStreamLocalPositionNED(), - // new MavlinkStreamGPSGlobalOrigin(), - // new MavlinkStreamServoOutputRaw(0), - // new MavlinkStreamServoOutputRaw(1), - // new MavlinkStreamServoOutputRaw(2), - // new MavlinkStreamServoOutputRaw(3), - // new MavlinkStreamHILControls(), - // new MavlinkStreamGlobalPositionSetpointInt(), - // new MavlinkStreamLocalPositionSetpoint(), - // new MavlinkStreamRollPitchYawThrustSetpoint(), - // new MavlinkStreamRollPitchYawRatesThrustSetpoint(), + new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), + new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), + new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), + new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), - // new MavlinkStreamAttitudeControls(), - // new MavlinkStreamNamedValueFloat(), - // new MavlinkStreamCameraCapture(), - // new MavlinkStreamDistanceSensor(), - // new MavlinkStreamViconPositionEstimate(), + new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), + new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), + new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), + new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), + new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), nullptr }; -- cgit v1.2.3 From 885efa2cfe3fd74dbf99b577e3ca486b154fd754 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 17 May 2014 20:58:27 +0200 Subject: fw pos control: landing: fix argument order --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 30 ++++++++++++++++------ 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 20f3fefcd..95165ecaa 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1047,11 +1047,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi altitude_desired_rel = math::max(relative_alt, L_altitude_rel); } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, + calculate_target_airspeed(airspeed_approach), eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), _pos_sp_triplet.current.alt + relative_alt, - false, math::radians(_parameters.pitch_limit_min), ground_speed); + ground_speed); } } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { @@ -1107,10 +1113,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); } else { - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(_parameters.airspeed_trim), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } } else { -- cgit v1.2.3 From a28a38005c6c9fdd43e6e7182b93d274d4f4369c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 19 May 2014 08:49:25 +0200 Subject: mtecs fix integrator --- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 29 +++------------------- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 18 ++++++++++++++ 2 files changed, 22 insertions(+), 25 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index f3dc9bcb2..047e4d335 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -51,27 +51,6 @@ namespace fwPosctrl using namespace control; -/* Integrator without limit */ -class BlockIntegralNoLimit: public SuperBlock -{ -public: -// methods - BlockIntegralNoLimit(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _y(0) {}; - virtual ~BlockIntegralNoLimit() {}; - float update(float input) { - setY(getY() + input * getDt()); - return getY(); - }; -// accessors - float getY() {return _y;} - void setY(float y) {_y = y;} -protected: -// attributes - float _y; /**< previous output */ -}; - /* An block which can be used to limit the output */ class BlockOutputLimiter: public SuperBlock { @@ -128,7 +107,7 @@ public: virtual ~BlockFFPILimited() {}; float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); } // accessors - BlockIntegralNoLimit &getIntegral() { return _integral; } + BlockIntegral &getIntegral() { return _integral; } float getKFF() { return _kFF.get(); } float getKP() { return _kP.get(); } float getKI() { return _kI.get(); } @@ -143,14 +122,14 @@ protected: float integralYPrevious = _integral.getY(); float output = calcUnlimitedOutput(inputValue, inputError); if(!outputLimiter.limit(output, difference) && - (((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) || - ((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) { + (((difference < 0) && (getKI() * getIntegral().getY() < 0)) || + ((difference > 0) && (getKI() * getIntegral().getY() > 0)))) { getIntegral().setY(integralYPrevious); } return output; } private: - BlockIntegralNoLimit _integral; + BlockIntegral _integral; BlockParamFloat _kFF; BlockParamFloat _kP; BlockParamFloat _kI; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 3a05f2c88..591257611 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -360,3 +360,21 @@ PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f); * @group mTECS */ PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f); + +/** + * Integrator Limit for Total Energy Rate Control + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f); + +/** + * Integrator Limit for Energy Distribution Rate Control + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f); -- cgit v1.2.3 From e92620b9b2d130c5ea3e9fabb91a9e74cba4f710 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 20 May 2014 23:15:58 +0200 Subject: rc_channels topic cleanup --- src/modules/mavlink/mavlink_messages.cpp | 1 - src/modules/sdlog2/sdlog2.c | 4 +- src/modules/sensors/sensors.cpp | 26 ++++++------ src/modules/uORB/topics/rc_channels.h | 68 ++++++++++++-------------------- 4 files changed, 41 insertions(+), 58 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 933478f56..401faf8a7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -45,7 +45,6 @@ #include #include -#include #include #include #include diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 70ce76806..b29c0e086 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1200,8 +1200,8 @@ int sdlog2_thread_main(int argc, char *argv[]) if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) { log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ - memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); - log_msg.body.log_RC.channel_count = buf.rc.chan_count; + memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.channel_count = buf.rc.channel_count; log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; LOGBUFFER_WRITE_AND_COUNT(RC); } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b268b1b36..319626d6e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1202,9 +1202,9 @@ Sensors::parameter_update_poll(bool forced) } #if 0 - printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]); - printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]); - printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled * 100), (int)(_rc.chan[1].scaled * 100)); + printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]); + printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]); + printf("MAN: %d %d\n", (int)(_rc.channels[0] * 100), (int)(_rc.channels[1] * 100)); fflush(stdout); usleep(5000); #endif @@ -1334,7 +1334,7 @@ float Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value) { if (_rc.function[func] >= 0) { - float value = _rc.chan[_rc.function[func]].scaled; + float value = _rc.channels[_rc.function[func]]; if (value < min_value) { return min_value; @@ -1355,7 +1355,7 @@ switch_pos_t Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv) { if (_rc.function[func] >= 0) { - float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; @@ -1376,7 +1376,7 @@ switch_pos_t Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv) { if (_rc.function[func] >= 0) { - float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; @@ -1468,25 +1468,25 @@ Sensors::rc_poll() * DO NOT REMOVE OR ALTER STEP 1! */ if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); + _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { - _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); + _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); } else { /* in the configured dead zone, output zero */ - _rc.chan[i].scaled = 0.0f; + _rc.channels[i] = 0.0f; } - _rc.chan[i].scaled *= _parameters.rev[i]; + _rc.channels[i] *= _parameters.rev[i]; /* handle any parameter-induced blowups */ - if (!isfinite(_rc.chan[i].scaled)) { - _rc.chan[i].scaled = 0.0f; + if (!isfinite(_rc.channels[i])) { + _rc.channels[i] = 0.0f; } } - _rc.chan_count = rc_input.channel_count; + _rc.channel_count = rc_input.channel_count; _rc.rssi = rc_input.rssi; _rc.signal_lost = signal_lost; _rc.timestamp = rc_input.timestamp_last_signal; diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 370c54442..829d8e57d 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,60 +42,44 @@ #include #include "../uORB.h" -/** - * The number of RC channel inputs supported. - * Current (Q4/2013) radios support up to 18 channels, - * leaving at a sane value of 16. - * This number can be greater then number of RC channels, - * because single RC channel can be mapped to multiple - * functions, e.g. for various mode switches. - */ -#define RC_CHANNELS_MAPPED_MAX 16 - /** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of - * the channel array chan[]. + * the channel array channels[]. */ enum RC_CHANNELS_FUNCTION { THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - POSCTL = 6, - LOITER = 7, - OFFBOARD_MODE = 8, - ACRO = 9, - FLAPS = 10, - AUX_1 = 11, - AUX_2 = 12, - AUX_3 = 13, - AUX_4 = 14, - AUX_5 = 15, - RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ + ROLL, + PITCH, + YAW, + MODE, + RETURN, + POSCTL, + LOITER, + OFFBOARD_MODE, + ACRO, + FLAPS, + AUX_1, + AUX_2, + AUX_3, + AUX_4, + AUX_5, + RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */ }; /** * @addtogroup topics * @{ */ - struct rc_channels_s { - - uint64_t timestamp; /**< In microseconds since boot time. */ - uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ - struct { - float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_MAPPED_MAX]; - uint8_t chan_count; /**< number of valid channels */ - - /*String array to store the names of the functions*/ - char function_name[RC_CHANNELS_FUNCTION_MAX][20]; - int8_t function[RC_CHANNELS_FUNCTION_MAX]; - uint8_t rssi; /**< Overall receive signal strength */ - bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ + uint64_t timestamp; /**< Timestamp in microseconds since boot time */ + uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */ + float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */ + uint8_t channel_count; /**< Number of valid channels */ + char function_name[RC_CHANNELS_FUNCTION_MAX][20]; /**< String array to store the names of the functions */ + int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */ + uint8_t rssi; /**< Receive signal strength index */ + bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */ }; /**< radio control channels. */ /** -- cgit v1.2.3 From 78637ff74b4c12edca0fac9ea8eb65f38993b49f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 21 May 2014 11:14:06 +0200 Subject: mavlink: publish attitude / rates setpoint in offboard control mode --- src/modules/mavlink/mavlink_receiver.cpp | 72 +++++++++++++++++++++++++++++--- src/modules/mavlink/mavlink_receiver.h | 7 ++++ 2 files changed, 74 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 72b9ee83a..561744755 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -102,16 +102,21 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _cmd_pub(-1), _flow_pub(-1), _offboard_control_sp_pub(-1), + _att_sp_pub(-1), + _rates_sp_pub(-1), _vicon_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _control_mode_sub(-1), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), _hil_local_alt0(0.0) { + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); memset(&hil_local_pos, 0, sizeof(hil_local_pos)); + memset(&_control_mode, 0, sizeof(_control_mode)); } MavlinkReceiver::~MavlinkReceiver() @@ -349,33 +354,33 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - uint8_t ml_mode = 0; + enum OFFBOARD_CONTROL_MODE ml_mode = OFFBOARD_CONTROL_MODE_DIRECT; bool ml_armed = false; switch (quad_motors_setpoint.mode) { case 0: - ml_armed = false; break; case 1: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; ml_armed = true; - break; case 2: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; ml_armed = true; - break; case 3: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + ml_armed = true; break; case 4: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; break; + default: + break; } offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; @@ -388,7 +393,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = static_cast(ml_mode); + offboard_control_sp.mode = ml_mode; offboard_control_sp.timestamp = hrt_absolute_time(); @@ -398,6 +403,63 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } else { orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } + + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + + if (_control_mode.flag_control_offboard_enabled) { + if (_control_mode.flag_control_position_enabled) { + // TODO + + } else if (_control_mode.flag_control_velocity_enabled) { + // TODO + + } else if (_control_mode.flag_control_attitude_enabled) { + /* attitude control */ + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + + att_sp.roll_body = offboard_control_sp.p1; + att_sp.pitch_body = offboard_control_sp.p2; + att_sp.yaw_body = offboard_control_sp.p3; + att_sp.thrust = offboard_control_sp.p4; + + att_sp.timestamp = hrt_absolute_time(); + + if (_att_sp_pub < 0) { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + + } else { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + } + + } else if (_control_mode.flag_control_rates_enabled) { + /* rates control */ + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); + + rates_sp.roll = offboard_control_sp.p1; + rates_sp.pitch = offboard_control_sp.p2; + rates_sp.yaw = offboard_control_sp.p3; + rates_sp.thrust = offboard_control_sp.p4; + + rates_sp.timestamp = hrt_absolute_time(); + + if (_rates_sp_pub < 0) { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + + } else { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); + } + + } else { + /* direct control */ + // TODO + } + } + } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..dc31b4c5a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -36,6 +36,7 @@ * MAVLink 1.0 uORB listener definition * * @author Lorenz Meier + * @author Anton Babushkin */ #pragma once @@ -44,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -120,6 +122,7 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; + struct vehicle_control_mode_s _control_mode; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; orb_advert_t _attitude_pub; @@ -134,10 +137,14 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; + orb_advert_t _att_sp_pub; + orb_advert_t _rates_sp_pub; orb_advert_t _vicon_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + + int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; -- cgit v1.2.3 From 2a354c2d10a49c65a05665a0d631ebe615f306bc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 24 May 2014 14:07:51 +0200 Subject: mtecs: fix usage of outputLimiter.limit --- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index 047e4d335..a7acd95de 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -63,6 +63,14 @@ public: _max(this, "MAX") {}; virtual ~BlockOutputLimiter() {}; + /* + * Imposes the limits given by _min and _max on value + * + * @param value is changed to be on the interval _min to _max + * @param difference if the value is changed this corresponds to the change of value * (-1) + * otherwise unchanged + * @return: true if the limit is applied, false otherwise + */ bool limit(float& value, float& difference) { float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); @@ -121,7 +129,7 @@ protected: float difference = 0.0f; float integralYPrevious = _integral.getY(); float output = calcUnlimitedOutput(inputValue, inputError); - if(!outputLimiter.limit(output, difference) && + if(outputLimiter.limit(output, difference) && (((difference < 0) && (getKI() * getIntegral().getY() < 0)) || ((difference > 0) && (getKI() * getIntegral().getY() > 0)))) { getIntegral().setY(integralYPrevious); -- cgit v1.2.3 From 56ac13aafbbf3e9875e9c17f82f687a2690033da Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 24 May 2014 18:04:38 +0200 Subject: introduce tecs status uorb message --- src/modules/uORB/Publication.cpp | 2 + src/modules/uORB/objects_common.cpp | 4 ++ src/modules/uORB/topics/tecs_status.h | 81 +++++++++++++++++++++++++++++++++++ 3 files changed, 87 insertions(+) create mode 100644 src/modules/uORB/topics/tecs_status.h diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 5a5981617..cd0b30dd6 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -47,6 +47,7 @@ #include "topics/vehicle_rates_setpoint.h" #include "topics/actuator_outputs.h" #include "topics/encoders.h" +#include "topics/tecs_status.h" namespace uORB { @@ -76,5 +77,6 @@ template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e..bae46e14d 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -199,3 +199,7 @@ ORB_DEFINE(encoders, struct encoders_s); #include "topics/estimator_status.h" ORB_DEFINE(estimator_status, struct estimator_status_report); + +#include "topics/tecs_status.h" +ORB_DEFINE(tecs_status, struct tecs_status_s); + diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h new file mode 100644 index 000000000..f3d33ec20 --- /dev/null +++ b/src/modules/uORB/topics/tecs_status.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_global_position.h + * Definition of the global fused WGS84 position uORB topic. + * + * @author Thomas Gubler + */ + +#ifndef TECS_STATUS_T_H_ +#define TECS_STATUS_T_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + + /** + * Internal values of the (m)TECS fixed wing speed alnd altitude control system + */ +struct tecs_status_s { + uint64_t timestamp; /**< timestamp, in microseconds since system start */ + + float altitudeSp; + float altitude; + float flightPathAngleSp; + float flightPathAngle; + float airspeedSp; + float airspeed; + float airspeedDerivativeSp; + float airspeedDerivative; + + float totalEnergyRateSp; + float totalEnergyRate; + float energyDistributionRateSp; + float energyDistributionRate; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(tecs_status); + +#endif -- cgit v1.2.3 From 48d884ec8fea039be8c750c9643bb476d8f8241d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 24 May 2014 18:05:31 +0200 Subject: mtecs: publish tecs status uorb message and small variable rename --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 38 ++++++++++++++++++++++----- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 5 ++++ 2 files changed, 36 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index c6301bcdb..94a614bc0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -51,6 +51,8 @@ mTecs::mTecs() : /* Parameters */ _mTecsEnabled(this, "ENABLED"), _airspeedMin(this, "FW_AIRSPD_MIN", false), + /* Publications */ + _status(&getPublications(), ORB_ID(tecs_status)), /* control blocks */ _controlTotalEnergy(this, "THR"), _controlEnergyDistribution(this, "PIT", true), @@ -97,6 +99,11 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); } + /* Write part of the status message */ + _status.altitudeSp = altitudeSp; + _status.altitude = altitude; + + /* use flightpath angle setpoint for total energy control */ return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode); } @@ -120,6 +127,13 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); } + /* Write part of the status message */ + _status.flightPathAngleSp = flightPathAngleSp; + _status.flightPathAngle = flightPathAngle; + _status.airspeedSp = airspeedSp; + _status.airspeed = airspeed; + + /* use longitudinal acceleration setpoint for total energy control */ return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode); } @@ -144,18 +158,18 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight airspeedDerivative = _airspeedDerivative.update(airspeed); } float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; - float airspeedSpDerivative = accelerationLongitudinalSp; - float airspeedSpDerivativeNorm = airspeedSpDerivative / CONSTANTS_ONE_G; - float airspeedSpDerivativeNormError = airspeedSpDerivativeNorm - airspeedDerivativeNorm; + float airspeedDerivativeSp = accelerationLongitudinalSp; + float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G; + float airspeedDerivativeNormErrorSp = airspeedDerivativeNormSp - airspeedDerivativeNorm; float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm; - float totalEnergyRateError = flightPathAngleError + airspeedSpDerivativeNormError; - float totalEnergyRateSp = flightPathAngleSp + airspeedSpDerivativeNorm; + float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormErrorSp; + float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp; float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate; float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm; - float energyDistributionRateError = flightPathAngleError - airspeedSpDerivativeNormError; - float energyDistributionRateSp = flightPathAngleSp - airspeedSpDerivativeNorm; + float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormErrorSp; + float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp; float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; /* Debug output */ @@ -188,6 +202,14 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; } + /* Write part of the status message */ + _status.airspeedDerivativeSp = airspeedDerivativeSp; + _status.airspeedDerivative = airspeedDerivative; + _status.totalEnergyRateSp = totalEnergyRateSp; + _status.totalEnergyRate = totalEnergyRate; + _status.energyDistributionRateSp = energyDistributionRateSp; + _status.energyDistributionRate = energyDistributionRate; + /** update control blocks **/ /* update total energy rate control block */ _throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle); @@ -203,6 +225,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight (double)accelerationLongitudinalSp, (double)airspeedDerivative); } + /* publish status messge */ + _status.update(); /* clean up */ _firstIterationAfterReset = false; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 147c108f3..376d39698 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -47,6 +47,8 @@ #include #include +#include +#include namespace fwPosctrl { @@ -97,6 +99,9 @@ protected: control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ control::BlockParamFloat _airspeedMin; /**< minimal airspeed */ + /* Publications */ + uORB::Publication _status; /**< publish internal values for logging */ + /* control blocks */ BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ -- cgit v1.2.3 From e87460b9f4d2a5dde2c0acd07fe3c727007b27c3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 24 May 2014 18:07:46 +0200 Subject: sdlog: log tecs status messages --- src/modules/sdlog2/sdlog2.c | 23 +++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 19 +++++++++++++++++++ 2 files changed, 42 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 577cadfbb..e1f3490a9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -84,6 +84,7 @@ #include #include #include +#include #include #include @@ -939,6 +940,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct telemetry_status_s telemetry; struct range_finder_report range_finder; struct estimator_status_report estimator_status; + struct tecs_status_s tecs_status; struct system_power_s system_power; struct servorail_status_s servorail_status; } buf; @@ -979,6 +981,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS0B_s log_GS0B; struct log_GS1A_s log_GS1A; struct log_GS1B_s log_GS1B; + struct log_TECS_s log_TECS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1010,6 +1013,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int telemetry_sub; int range_finder_sub; int estimator_status_sub; + int tecs_status_sub; int system_power_sub; int servorail_status_sub; } subs; @@ -1037,6 +1041,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); + subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); @@ -1488,6 +1493,24 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ESTM); } + /* --- TECS STATUS --- */ + if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) { + log_msg.msg_type = LOG_TECS_MSG; + log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; + log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; + log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; + log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; + log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; + log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; + log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; + log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; + log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; + log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate; + log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp; + log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate; + LOGBUFFER_WRITE_AND_COUNT(TECS); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index f4d88f079..93ed0cdfc 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -346,6 +346,24 @@ struct log_GS1B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; +/* --- TECS - TECS STAUS --- */ +#define LOG_TECS_MSG 30 +struct log_TECS_s { + float altitudeSp; + float altitude; + float flightPathAngleSp; + float flightPathAngle; + float airspeedSp; + float airspeed; + float airspeedDerivativeSp; + float airspeedDerivative; + + float totalEnergyRateSp; + float totalEnergyRate; + float energyDistributionRateSp; + float energyDistributionRate; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -401,6 +419,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(TECS, "ffffffffffff", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From c7bf00562d685b513e1720f558fee5840aca151b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 08:22:54 +0200 Subject: commander: Enforce (in presence of power sensing) that a) system is not purely servo rail powered and b) power rail voltage is higher than 4.5V on the main avionics rail --- src/modules/commander/commander.cpp | 27 ++++++++++++++++++++++++++ src/modules/commander/state_machine_helper.cpp | 22 +++++++++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 ++ 3 files changed, 51 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c0628a16..55c74fdff 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -717,6 +718,9 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = hrt_absolute_time(); + status.condition_power_input_valid = true; + status.avionics_power_rail_voltage = -1.0f; + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -853,6 +857,11 @@ int commander_thread_main(int argc, char *argv[]) struct position_setpoint_triplet_s pos_sp_triplet; memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + /* Subscribe to system power */ + int system_power_sub = orb_subscribe(ORB_ID(system_power)); + struct system_power_s system_power; + memset(&system_power, 0, sizeof(system_power)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -933,6 +942,24 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); } + orb_check(system_power_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(system_power), system_power_sub, &system_power); + + if (hrt_elapsed_time(&system_power.timestamp) < 200000) { + if (system_power.servo_valid && + !system_power.brick_valid && + !system_power.usb_connected) { + /* flying only on servo rail, this is unsafe */ + status.condition_power_input_valid = false; + } else { + status.condition_power_input_valid = true; + status.avionics_power_rail_voltage = system_power.voltage5V_v; + } + } + } + check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); /* update safety topic */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87309b238..818974648 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -144,6 +144,28 @@ arming_state_transition(struct vehicle_status_s *status, /// current valid_transition = false; } + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + } + + valid_transition = false; + } + + // Fail transition if power levels on the avionics rail + // are insufficient + if ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.5f)) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + } + + valid_transition = false; + } + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { new_arming_state = ARMING_STATE_STANDBY_ERROR; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ea20a317a..2ed0d6ad4 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -179,6 +179,8 @@ struct vehicle_status_s { bool condition_local_altitude_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ + bool condition_power_input_valid; /**< set if input power is valid */ + float avionics_power_rail_voltage; /**< voltage of the avionics power rail */ bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ -- cgit v1.2.3 From 0cddae02ab9c9ac52bde196b936b549f32435fb4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 May 2014 15:45:01 +0200 Subject: fw: resolve an issue when the aircraft was climbing before landing --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 4f40682ea..f23719794 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -977,7 +977,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance); + float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); @@ -1032,10 +1032,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* intersect glide slope: * minimize speed to approach speed * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope + * also if the system captures the slope it should stay + * on the slope (bool land_onslope) + * if current position is below slope -10m continue at previous wp altitude + * until the intersection with slope * */ float altitude_desired_rel = relative_alt; - if (relative_alt > landing_slope_alt_rel_desired - 10.0f) { + if (relative_alt > landing_slope_alt_rel_desired - 10.0f || land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { @@ -1044,7 +1047,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } else { /* continue horizontally */ - altitude_desired_rel = math::max(relative_alt, L_altitude_rel); + altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt; } tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, -- cgit v1.2.3 From 77f0e8950f90d5304278dd6c8f47c886dd4ed572 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 May 2014 15:45:20 +0200 Subject: mtecs: rename variable --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 94a614bc0..087e1b476 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -160,15 +160,15 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; float airspeedDerivativeSp = accelerationLongitudinalSp; float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G; - float airspeedDerivativeNormErrorSp = airspeedDerivativeNormSp - airspeedDerivativeNorm; + float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm; float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm; - float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormErrorSp; + float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError; float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp; float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate; float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm; - float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormErrorSp; + float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError; float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp; float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; -- cgit v1.2.3 From 032fe389a5630a22c9da4642d79b3fae05a213f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:34:11 +0200 Subject: systemlib: Add circuit breakers --- src/modules/systemlib/circuit_breaker.c | 93 +++++++++++++++++++++++++++++++++ src/modules/systemlib/circuit_breaker.h | 55 +++++++++++++++++++ src/modules/systemlib/module.mk | 5 +- 3 files changed, 151 insertions(+), 2 deletions(-) create mode 100644 src/modules/systemlib/circuit_breaker.c create mode 100644 src/modules/systemlib/circuit_breaker.h diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c new file mode 100644 index 000000000..b59531d69 --- /dev/null +++ b/src/modules/systemlib/circuit_breaker.c @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file circuit_breaker.c + * + * Circuit breaker parameters. + * Analog to real aviation circuit breakers these parameters + * allow to disable subsystems. They are not supported as standard + * operation procedure and are only provided for development purposes. + * To ensure they are not activated accidentally, the associated + * parameter needs to set to the key (magic). + */ + +#include +#include + +/** + * Circuit breaker for power supply check + * + * Setting this parameter to 894281 will disable the power valid + * checks in the commander. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 894281 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); + +/** + * Circuit breaker for rate controller output + * + * Setting this parameter to 140253 will disable the rate + * controller uORB publication. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 140253 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); + +/** + * Circuit breaker for IO safety + * + * Setting this parameter to 894281 will disable IO safety. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 22027 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); + +bool circuit_breaker_enabled(const char* breaker, int32_t magic) +{ + int32_t val; + (void)param_get(param_find(breaker), val); + + return (val == magic); +} + diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h new file mode 100644 index 000000000..86439e2a5 --- /dev/null +++ b/src/modules/systemlib/circuit_breaker.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file circuit_breaker.h + * + * Circuit breaker functionality. + */ + +#ifndef CIRCUIT_BREAKER_H_ +#define CIRCUIT_BREAKER_H_ + +#define CBRK_SUPPLY_CHK_KEY 894281 +#define CBRK_RATE_CTRL_KEY 140253 +#define CBRK_IO_SAFETY_KEY 22027 + +#include + +__BEGIN_DECLS + +__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); + +__END_DECLS + +#endif /* CIRCUIT_BREAKER_H_ */ diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 3953b757d..147903aa0 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -52,5 +52,6 @@ SRCS = err.c \ rc_check.c \ otp.c \ board_serial.c \ - pwm_limit/pwm_limit.c + pwm_limit/pwm_limit.c \ + circuit_breaker.c -- cgit v1.2.3 From 064329dd099819b0f0b8a9a7bc0233440fdf6df1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:34:32 +0200 Subject: commander: put circuit breaker into effect --- src/modules/commander/commander.cpp | 7 ++++++ src/modules/commander/state_machine_helper.cpp | 32 +++++++++++++++----------- 2 files changed, 25 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 55c74fdff..71067ac4f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -721,6 +722,9 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = true; status.avionics_power_rail_voltage = -1.0f; + // CIRCUIT BREAKERS + status.circuit_breaker_engaged_power_check = false; + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -907,6 +911,9 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); + + status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status_changed = true; /* re-check RC calibration */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 818974648..84f4d03af 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -144,26 +144,30 @@ arming_state_transition(struct vehicle_status_s *status, /// current valid_transition = false; } - // Fail transition if power is not good - if (!status->condition_power_input_valid) { + // Perform power checks only if circuit breaker is not + // engaged for these checks + if (!status->circuit_breaker_engaged_power_check) { + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + } - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + valid_transition = false; } - valid_transition = false; - } + // Fail transition if power levels on the avionics rail + // are insufficient + if ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.5f)) { - // Fail transition if power levels on the avionics rail - // are insufficient - if ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.5f)) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + } - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + valid_transition = false; } - - valid_transition = false; } } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { -- cgit v1.2.3 From 55ad5588a88bc4957583067beeffe20aa2fcd442 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:35:16 +0200 Subject: system status topic: Add support for circuit breaker --- src/modules/uORB/topics/vehicle_status.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 2ed0d6ad4..85962883d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -208,6 +208,8 @@ struct vehicle_status_s { uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; + + bool circuit_breaker_engaged_power_check; }; /** -- cgit v1.2.3 From 77365188ada556c5953f6f026566d4912e8c6e76 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:35:37 +0200 Subject: mc_att_control: Support circuit breakers --- src/modules/mc_att_control/mc_att_control_main.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 20e016da3..19c10198c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include @@ -123,6 +124,8 @@ private: orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ @@ -267,6 +270,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _v_rates_sp_pub(-1), _actuators_0_pub(-1), + _actuators_0_circuit_breaker_enabled(false), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) @@ -402,6 +407,8 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); + return OK; } @@ -840,11 +847,13 @@ MulticopterAttitudeControl::task_main() _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + if (!_actuators_0_circuit_breaker_enabled) { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } } } } -- cgit v1.2.3 From 8ea5fd20c1c268c6baf145606186592bf5bd3699 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:36:05 +0200 Subject: px4io driver: Add support for circuit breakers --- src/drivers/px4io/px4io.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 972f45148..c6acfe6be 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include @@ -1010,6 +1011,19 @@ PX4IO::task_main() } } + int32_t safety_param_val; + param_t safety_param = param_find("RC_FAILS_THR"); + + if (safety_param != PARAM_INVALID) { + + param_get(safety_param, &safety_param_val); + + if (safety_param_val == PX4IO_FORCE_SAFETY_MAGIC) { + /* disable IO safety if circuit breaker asked for it */ + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, safety_param_val); + } + } + } } -- cgit v1.2.3 From bb0cae56a0bca879bd3f1720a92d1bf0ca03eda2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 26 May 2014 08:42:08 +0200 Subject: fix typo in comment --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 93ed0cdfc..8a0a0ad45 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -346,7 +346,7 @@ struct log_GS1B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; -/* --- TECS - TECS STAUS --- */ +/* --- TECS - TECS STATUS --- */ #define LOG_TECS_MSG 30 struct log_TECS_s { float altitudeSp; -- cgit v1.2.3 From 10e2a6696976a5210d826dd5826b87db76990eaa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 27 May 2014 08:37:59 +0200 Subject: fw pos control: landing: continue horizontally instead of climbing if just below slope --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f23719794..e3b6eb261 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1031,14 +1031,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* intersect glide slope: * minimize speed to approach speed - * if current position is higher or within 10m of slope follow the glide slope + * if current position is higher than the slope follow the glide slope (sink to the + * glide slope) * also if the system captures the slope it should stay * on the slope (bool land_onslope) - * if current position is below slope -10m continue at previous wp altitude + * if current position is below the slope continue at previous wp altitude * until the intersection with slope * */ float altitude_desired_rel = relative_alt; - if (relative_alt > landing_slope_alt_rel_desired - 10.0f || land_onslope) { + if (relative_alt > landing_slope_alt_rel_desired || land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { -- cgit v1.2.3 From 16ca6c1605e0864d37cce4cfdbe790df5746bb38 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 May 2014 14:52:42 +0200 Subject: position_estimator_inav: don't change local z on first time ref initialization --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index fdc3233e0..f236d546b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -622,8 +622,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; - /* update baro offset */ - baro_offset -= z_est[0]; /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; @@ -631,18 +629,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) x_est[2] = accel_NED[0]; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; - z_est[0] = 0.0f; y_est[2] = accel_NED[1]; local_pos.ref_lat = lat; local_pos.ref_lon = lon; - local_pos.ref_alt = alt; + local_pos.ref_alt = alt + z_est[0]; local_pos.ref_timestamp = t; /* initialize projection */ map_projection_init(&ref, lat, lon); warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", lat, lon, alt); } } -- cgit v1.2.3 From ed6c2a5168ca891f20594687acfd3c6bbf7e1cf9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 27 May 2014 21:56:32 +0200 Subject: commander and navigator: lot's of changes, failsafe handling in commander, navigator only for execution (WIP) --- src/drivers/blinkm/blinkm.cpp | 5 +- src/drivers/gps/mtk.cpp | 4 +- src/modules/commander/commander.cpp | 472 ++++++++++----------- src/modules/commander/state_machine_helper.cpp | 414 ++++-------------- src/modules/commander/state_machine_helper.h | 16 +- src/modules/mavlink/mavlink_messages.cpp | 86 ++-- src/modules/navigator/navigator_main.cpp | 240 +---------- src/modules/systemlib/state_table.h | 28 +- src/modules/uORB/topics/mission_result.h | 1 + .../uORB/topics/position_setpoint_triplet.h | 15 +- src/modules/uORB/topics/vehicle_status.h | 35 +- 11 files changed, 411 insertions(+), 905 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 5c502f682..98c491ce6 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -655,13 +655,14 @@ BlinkM::led() /* indicate main control state */ if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; - else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) + /* TODO: add other Auto modes */ + else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION) led_color_4 = LED_BLUE; else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; - else + else led_color_4 = LED_OFF; led_color_5 = led_color_4; } diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index ea4e95fb2..41716cd97 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -251,8 +251,8 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->lon = 0; // Indicate this data is not usable and bail out - _gps_position->eph_m = 1000.0f; - _gps_position->epv_m = 1000.0f; + _gps_position->eph = 1000.0f; + _gps_position->epv = 1000.0f; _gps_position->fix_type = 0; return; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 77d810a81..908384bb6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1,11 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes - * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,8 +33,13 @@ /** * @file commander.cpp - * Main system state machine implementation. + * Main fail-safe handling. * + * @author Petri Tanskanen + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin */ #include @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -87,6 +88,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -387,103 +389,80 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { - /* result of the command */ - enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } - /* only handle high-priority commands here */ + /* result of the command */ + enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - uint8_t base_mode = (uint8_t) cmd->param1; - uint8_t custom_main_mode = (uint8_t) cmd->param2; + uint8_t base_mode = (uint8_t)cmd->param1; + uint8_t custom_main_mode = (uint8_t)cmd->param2; - /* set HIL state */ - hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); - - /* if HIL got enabled, reset battery status state */ - if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { - /* reset the arming mode to disarmed */ - arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); - } + transition_result_t arming_ret = TRANSITION_NOT_CHANGED; - // Transition the arming state - transition_result_t arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + transition_result_t main_ret = TRANSITION_NOT_CHANGED; - /* set main state */ - transition_result_t main_res = TRANSITION_DENIED; + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); - if (hil_ret == OK) { - ret = true; - } - - // Transition the arming state - arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - - if (arming_res == TRANSITION_CHANGED) { - ret = true; - } + // Transition the arming state + arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); - } + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_ret = main_state_transition(status, MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_res = main_state_transition(status, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_res = main_state_transition(status, MAIN_STATE_ACRO); + main_ret = main_state_transition(status, MAIN_STATE_ACRO); } - } else { + } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status, MAIN_STATE_MANUAL); } } } - } - if (main_res == TRANSITION_CHANGED) { - ret = true; - } + if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) { + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; - if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } - break; - } case VEHICLE_CMD_COMPONENT_ARM_DISARM: { // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. @@ -502,10 +481,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (arming_res == TRANSITION_DENIED) { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } } } @@ -516,14 +495,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAVIGATION_STATE_LOITER; + status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAVIGATION_STATE_MISSION; + status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); @@ -531,6 +510,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; +#if 0 /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command @@ -538,15 +518,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { /* reject parachute depoyment not armed */ - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } break; +#endif case VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; @@ -560,10 +541,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->timestamp = hrt_absolute_time(); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } else { @@ -574,10 +555,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->timestamp = hrt_absolute_time(); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) { warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); @@ -609,13 +590,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, result); + answer_command(*cmd, cmd_result); } /* send any requested ACKs */ - if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* send acknowledge command */ // XXX TODO } @@ -682,7 +663,7 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAVIGATION_STATE_NONE; + status.set_nav_state = NAVIGATION_STATE_MANUAL; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; status.failsafe_state = FAILSAFE_STATE_NORMAL; @@ -772,6 +753,11 @@ int commander_thread_main(int argc, char *argv[]) safety.safety_switch_available = false; safety.safety_off = false; + /* Subscribe to mission result topic */ + int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + struct mission_result_s mission_result; + memset(&mission_result, 0, sizeof(mission_result)); + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -849,6 +835,13 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); + transition_result_t arming_ret; + + /* check which state machines for changes, clear "changed" flag */ + bool arming_state_changed = false; + bool main_state_changed = false; + bool failsafe_state_changed = false; + while (!thread_should_exit) { if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { @@ -856,6 +849,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + arming_ret = TRANSITION_NOT_CHANGED; + + /* update parameters */ orb_check(param_changed_sub, &updated); @@ -935,6 +931,7 @@ int commander_thread_main(int argc, char *argv[]) if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + arming_state_changed = true; } } } @@ -1131,12 +1128,21 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + + if (arming_ret == TRANSITION_CHANGED) { + warnx("changed 1"); + arming_state_changed = true; + } } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); - } + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + if (arming_ret == TRANSITION_CHANGED) { + warnx("changed 2"); + arming_state_changed = true; + } + } status_changed = true; } @@ -1144,11 +1150,18 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { - // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + /* TODO: check for sensors */ + warnx("arming status1: %d", status.arming_state); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + warnx("arming status2: %d", status.arming_state); + + if (arming_ret == TRANSITION_CHANGED) { + warnx("changed"); + arming_state_changed = true; + } } else { - // XXX: Add emergency stuff if sensors are lost + /* TODO: Add emergency stuff if sensors are lost */ } @@ -1167,6 +1180,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); } + orb_check(mission_result_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + } + /* start RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ @@ -1184,11 +1203,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - transition_result_t arming_res; // store all transitions results here - - /* arm/disarm by RC */ - arming_res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && @@ -1199,7 +1213,11 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed); + if (arming_ret == TRANSITION_CHANGED) { + warnx("stick 1"); + arming_state_changed = true; + } stick_off_counter = 0; } else { @@ -1221,7 +1239,11 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + if (arming_ret == TRANSITION_CHANGED) { + warnx("stick 2"); + arming_state_changed = true; + } } stick_on_counter = 0; @@ -1234,23 +1256,25 @@ int commander_thread_main(int argc, char *argv[]) stick_on_counter = 0; } - if (arming_res == TRANSITION_CHANGED) { + if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + arming_state_changed = true; - } else if (arming_res == TRANSITION_DENIED) { + } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* recover from failsafe */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - } + + // if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + // /* recover from failsafe */ + // (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + // } /* evaluate the main state machine according to mode switches */ transition_result_t main_res = set_main_state_rc(&status, &sp_man); @@ -1258,102 +1282,41 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on mode change only if armed, blink LED always */ if (main_res == TRANSITION_CHANGED) { tune_positive(armed.armed); + main_state_changed = true; } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } - /* set navigation state */ - /* RETURN switch, overrides MISSION switch */ - if (sp_man.return_switch == SWITCH_POS_ON) { - /* switch to RTL if not already landed after RTL and home position set */ - status.set_nav_state = NAVIGATION_STATE_RTL; - - - } else { - - /* LOITER switch */ - if (sp_man.loiter_switch == SWITCH_POS_ON) { - /* stick is in LOITER position */ - status.set_nav_state = NAVIGATION_STATE_LOITER; - - } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { - /* stick is in MISSION position */ - status.set_nav_state = NAVIGATION_STATE_MISSION; - - } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && - pos_sp_triplet.nav_state == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - status.set_nav_state = NAVIGATION_STATE_MISSION; - } - } - } else { if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } + } - if (armed.armed) { - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO); - - if (auto_res == TRANSITION_NOT_CHANGED) { - last_auto_state_valid = hrt_absolute_time(); - } - - /* still invalid state after the timeout interval, execute failsafe */ - if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (auto_res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - } - } else { - status.set_nav_state = NAVIGATION_STATE_MISSION; - } + if (armed.armed) { + /* if RC is lost, switch to RTL_RC or Termination unless a mission is running + * and not yet finished) */ + if (status.rc_signal_lost + && !(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { + /* if we have a global position, we can switch to RTL, if not, we can try to land */ + if (status.condition_global_position_valid) { + status.failsafe_state = FAILSAFE_STATE_RTL_RC; } else { - /* failsafe for manual modes */ - transition_result_t manual_res = TRANSITION_DENIED; - - if (!status.condition_landed) { - /* vehicle is not landed, try to perform RTL */ - manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - } - - if (manual_res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate) or not wanted, try LAND */ - manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (manual_res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - } - } else { - status.set_nav_state = NAVIGATION_STATE_RTL; - } - } - - } else { - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* reset failsafe when disarmed */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + status.failsafe_state = FAILSAFE_STATE_LAND; } } - } - // TODO remove this hack - /* flight termination in manual mode if assist switch is on POSCTL position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) { - if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { - tune_positive(armed.armed); - } + /* if the data link is lost, switch to RTL_DL or Termination */ + /* TODO: add this */ + + } else { + /* reset failsafe when disarmed */ + status.failsafe_state = FAILSAFE_STATE_NORMAL; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1369,11 +1332,6 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check which state machines for changes, clear "changed" flag */ - bool arming_state_changed = check_arming_state_changed(); - bool main_state_changed = check_main_state_changed(); - bool failsafe_state_changed = check_failsafe_state_changed(); - hrt_abstime t1 = hrt_absolute_time(); /* print new state */ @@ -1408,18 +1366,24 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; } + arming_state_changed = false; } was_armed = armed.armed; + /* now set nav state according to failsafe and main state */ + set_nav_state(&status); + if (main_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + main_state_changed = false; } if (failsafe_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); + failsafe_state_changed = false; } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ @@ -1608,6 +1572,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin switch (sp_man->mode_switch) { case SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; + warnx("NONE"); break; case SWITCH_POS_OFF: // MANUAL @@ -1648,14 +1613,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_ON: // AUTO - res = main_state_transition(status, MAIN_STATE_AUTO); + res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // else fallback to ALTCTL (POSCTL likely will not work too) - print_reject_mode(status, "AUTO"); res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -1675,85 +1639,93 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } void - set_control_mode() { - /* set vehicle_control_mode according to main state and failsafe state */ + /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; + /* TODO: check this */ control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; - control_mode.flag_control_termination_enabled = false; - - /* set this flag when navigator should act */ - bool navigator_enabled = false; - - switch (status.failsafe_state) { - case FAILSAFE_STATE_NORMAL: - switch (status.main_state) { - case MAIN_STATE_MANUAL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = status.is_rotary_wing; - control_mode.flag_control_attitude_enabled = status.is_rotary_wing; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_ALTCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_POSCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: - navigator_enabled = true; - break; + switch (status.set_nav_state) { + case NAVIGATION_STATE_MANUAL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = status.is_rotary_wing; + control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; - case MAIN_STATE_ACRO: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + case NAVIGATION_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; - default: - break; - } + case NAVIGATION_STATE_ALTCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_POSCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_RTL: - navigator_enabled = true; + case NAVIGATION_STATE_AUTO_MISSION: + case NAVIGATION_STATE_AUTO_LOITER: + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTL_RC: + case NAVIGATION_STATE_AUTO_RTL_DL: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_LAND: - navigator_enabled = true; + case NAVIGATION_STATE_LAND: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + /* in failsafe LAND mode position may be not available */ + control_mode.flag_control_position_enabled = status.condition_local_position_valid; + control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_TERMINATION: + case NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; @@ -1769,21 +1741,6 @@ set_control_mode() default: break; } - - /* navigator has control, set control mode flags according to nav state*/ - if (navigator_enabled) { - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - - /* in failsafe LAND mode position may be not available */ - control_mode.flag_control_position_enabled = status.condition_local_position_valid; - control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; - - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - } } void @@ -1927,7 +1884,6 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ - // XXX disable interrupts in arming_state_transition if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6f0e9794a..214480079 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +34,9 @@ /** * @file state_machine_helper.cpp * State machine helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes */ #include @@ -59,30 +60,20 @@ #include "state_machine_helper.h" #include "commander_helper.h" -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -static bool arming_state_changed = true; -static bool main_state_changed = true; -static bool failsafe_state_changed = true; - // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, - { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, - { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get it's textual representation @@ -165,7 +156,6 @@ arming_state_transition(struct vehicle_status_s *status, /// current armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; - arming_state_changed = true; } } @@ -199,69 +189,58 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet } } -bool -check_arming_state_changed() -{ - if (arming_state_changed) { - arming_state_changed = false; - return true; - - } else { - return false; - } -} - transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; - /* transition may be denied even if requested the same state because conditions may be changed */ + /* transition may be denied even if the same state is requested because conditions may have changed */ switch (new_main_state) { case MAIN_STATE_MANUAL: - ret = TRANSITION_CHANGED; - break; - case MAIN_STATE_ACRO: ret = TRANSITION_CHANGED; break; case MAIN_STATE_ALTCTL: - /* need at minimum altitude estimate */ + /* TODO: add this for fixedwing as well */ if (!status->is_rotary_wing || (status->condition_local_altitude_valid || status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } - break; case MAIN_STATE_POSCTL: - /* need at minimum local position estimate */ if (status->condition_local_position_valid || status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } - break; - case MAIN_STATE_AUTO: - + case MAIN_STATE_AUTO_MISSION: + case MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } + break; + case MAIN_STATE_AUTO_RTL: + /* need global position and home position */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } break; - } + case MAIN_STATE_MAX: + default: + break; + } if (ret == TRANSITION_CHANGED) { if (status->main_state != new_main_state) { status->main_state = new_main_state; - main_state_changed = true; - } else { ret = TRANSITION_NOT_CHANGED; } @@ -270,70 +249,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta return ret; } -bool -check_main_state_changed() -{ - if (main_state_changed) { - main_state_changed = false; - return true; - - } else { - return false; - } -} - -bool -check_failsafe_state_changed() -{ - if (failsafe_state_changed) { - failsafe_state_changed = false; - return true; - - } else { - return false; - } -} - /** -* Transition from one hil state to another -*/ -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) + * Transition from one hil state to another + */ +transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - bool valid_transition = false; - int ret = ERROR; + transition_result_t ret = TRANSITION_DENIED; if (current_status->hil_state == new_state) { - valid_transition = true; + ret = TRANSITION_NOT_CHANGED; } else { - switch (new_state) { - case HIL_STATE_OFF: - /* we're in HIL and unexpected things can happen if we disable HIL now */ mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); - valid_transition = false; - + ret = TRANSITION_DENIED; break; case HIL_STATE_ON: - if (current_status->arming_state == ARMING_STATE_INIT || current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - - // Disable publication of all attached sensors - + /* Disable publication of all attached sensors */ /* list directory */ DIR *d; d = opendir("/dev"); if (d) { - struct dirent *direntry; char devname[24]; @@ -388,288 +332,98 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } - closedir(d); + ret = TRANSITION_CHANGED; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + } else { /* failed opening dir */ - warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); - return 1; + mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); + ret = TRANSITION_DENIED; } + } else { + mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); + ret = TRANSITION_DENIED; } - break; default: - warnx("Unknown hil state"); + warnx("Unknown HIL state"); break; } } - if (valid_transition) { + if (ret = TRANSITION_CHANGED) { current_status->hil_state = new_state; - current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - // XXX also set lockdown here - - ret = OK; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); } - return ret; } /** -* Transition from one failsafe state to another -*/ -transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) + * Check failsafe and main status and set navigation status for navigator accordingly + */ +void set_nav_state(struct vehicle_status_s *status) { - transition_result_t ret = TRANSITION_DENIED; - - /* transition may be denied even if requested the same state because conditions may be changed */ - if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { - /* transitions from TERMINATION to other states not allowed */ - if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { - ret = TRANSITION_NOT_CHANGED; - } - - } else { - switch (new_failsafe_state) { - case FAILSAFE_STATE_NORMAL: - /* always allowed (except from TERMINATION state) */ - ret = TRANSITION_CHANGED; + switch (status->failsafe_state) { + case FAILSAFE_STATE_NORMAL: + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case MAIN_STATE_MANUAL: + status->set_nav_state = NAVIGATION_STATE_MANUAL; break; - case FAILSAFE_STATE_RTL: - - /* global position and home position required for RTL */ - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->set_nav_state = NAVIGATION_STATE_RTL; - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_ALTCTL: + status->set_nav_state = NAVIGATION_STATE_ALTCTL; + break; + case MAIN_STATE_POSCTL: + status->set_nav_state = NAVIGATION_STATE_POSCTL; break; - case FAILSAFE_STATE_LAND: + case MAIN_STATE_AUTO_MISSION: + status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; + break; - /* at least relative altitude estimate required for landing */ - if (status->condition_local_altitude_valid || status->condition_global_position_valid) { - status->set_nav_state = NAVIGATION_STATE_LAND; - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_AUTO_LOITER: + status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; + break; + case MAIN_STATE_AUTO_RTL: + status->set_nav_state = NAVIGATION_STATE_AUTO_RTL; break; - case FAILSAFE_STATE_TERMINATION: - /* always allowed */ - ret = TRANSITION_CHANGED; + case MAIN_STATE_ACRO: + status->set_nav_state = NAVIGATION_STATE_ACRO; break; default: break; } + break; - if (ret == TRANSITION_CHANGED) { - if (status->failsafe_state != new_failsafe_state) { - status->failsafe_state = new_failsafe_state; - failsafe_state_changed = true; - - } else { - ret = TRANSITION_NOT_CHANGED; - } - } - } + case FAILSAFE_STATE_RTL_RC: + status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC; + break; - return ret; -} + case FAILSAFE_STATE_RTL_DL: + status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL; + break; + case FAILSAFE_STATE_LAND: + status->set_nav_state = NAVIGATION_STATE_LAND; + break; + case FAILSAFE_STATE_TERMINATION: + status->set_nav_state = NAVIGATION_STATE_TERMINATION; + break; -// /* -// * Wrapper functions (to be used in the commander), all functions assume lock on current_status -// */ - -// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR -// * -// * START SUBSYSTEM/EMERGENCY FUNCTIONS -// * */ - -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was removed something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was disabled something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - - -///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ -// -//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -//{ -// int ret = 1; -// -//// /* Switch on HIL if in standby and not already in HIL mode */ -//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) -//// && !current_status->flag_hil_enabled) { -//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { -//// /* Enable HIL on request */ -//// current_status->flag_hil_enabled = true; -//// ret = OK; -//// state_machine_publish(status_pub, current_status, mavlink_fd); -//// publish_armed_status(current_status); -//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); -//// -//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && -//// current_status->flag_fmu_armed) { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") -//// -//// } else { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") -//// } -//// } -// -// /* switch manual / auto */ -// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { -// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { -// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { -// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { -// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); -// } -// -// /* vehicle is disarmed, mode requests arming */ -// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only arm in standby state */ -// // XXX REMOVE -// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); -// ret = OK; -// printf("[cmd] arming due to command request\n"); -// } -// } -// -// /* vehicle is armed, mode requests disarming */ -// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only disarm in ground ready */ -// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); -// ret = OK; -// printf("[cmd] disarming due to command request\n"); -// } -// } -// -// /* NEVER actually switch off HIL without reboot */ -// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { -// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); -// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); -// ret = ERROR; -// } -// -// return ret; -//} + default: + break; + } +} diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0ddd4f05a..594bf23a1 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -56,25 +56,17 @@ typedef enum { } transition_result_t; -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); - bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); -bool check_arming_state_changed(); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -bool check_main_state_changed(); - transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); -bool check_navigation_state_changed(); - -bool check_failsafe_state_changed(); - -void set_navigation_state_changed(); +transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); +void set_nav_state(struct vehicle_status_s *status); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 70087cf3e..31c0c8f79 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -118,51 +118,71 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND - || pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) { - /* use main state when navigator is not active */ - if (status->main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - - } else if (status->main_state == MAIN_STATE_ALTCTL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + switch (status->set_nav_state) { - } else if (status->main_state == MAIN_STATE_POSCTL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; - - } else if (status->main_state == MAIN_STATE_AUTO) { - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + case NAVIGATION_STATE_MANUAL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; - } else if (status->main_state == MAIN_STATE_ACRO) { + case NAVIGATION_STATE_ACRO: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; - } - - } else { - /* use navigation state when navigator is active */ - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + break; - if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + case NAVIGATION_STATE_ALTCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + case NAVIGATION_STATE_POSCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { + case NAVIGATION_STATE_AUTO_MISSION: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + case NAVIGATION_STATE_AUTO_LOITER: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; + + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTL_RC: + case NAVIGATION_STATE_AUTO_RTL_DL: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + case NAVIGATION_STATE_LAND: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - } + break; + + case NAVIGATION_STATE_TERMINATION: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 74694447a..f30cd9a94 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ /** * @file navigator_main.cpp - * Implementation of the main navigation state machine. * * Handles mission items, geo fencing and failsafe navigation behavior. * Published the position setpoint triplet for the position controller. @@ -75,7 +74,6 @@ #include #include -#include #include #include #include @@ -102,7 +100,7 @@ static const int ERROR = -1; */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator : public StateTable +class Navigator { public: /** @@ -116,7 +114,7 @@ public: ~Navigator(); /** - * Start the navigator task. + * Start the navigator task. * * @return OK on success. */ @@ -200,24 +198,6 @@ private: param_t takeoff_alt; } _parameter_handles; /**< handles for parameters */ - enum Event { - EVENT_NONE_REQUESTED, - EVENT_LOITER_REQUESTED, - EVENT_MISSION_REQUESTED, - EVENT_RTL_REQUESTED, - EVENT_TAKEN_OFF, - EVENT_LANDED, - EVENT_MISSION_CHANGED, - EVENT_HOME_POSITION_CHANGED, - EVENT_MISSION_ITEM_REACHED, - MAX_EVENT - }; /**< possible events that can be thrown at state machine */ - - /** - * State machine transition table - */ - static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; - /** * Update our local parameter cache. */ @@ -268,41 +248,6 @@ private: */ void task_main(); - /** - * Functions that are triggered when a new state is entered. - */ - bool start_none_on_ground(); - bool start_none_in_air(); - bool start_auto_on_ground(); - bool start_loiter(); - bool start_mission(); - bool advance_mission(); - bool start_rtl(); - bool advance_rtl(); - bool start_land(); - - /** - * Reset all "mission item reached" flags. - */ - void reset_reached(); - - /** - * Check if current mission item has been reached. - */ - bool check_mission_item_reached(); - - /** - * Set mission items by accessing the mission class. - * If failing, tell the state machine to loiter. - */ - bool set_mission_items(); - - /** - * Set a RTL item by accessing the RTL class. - * If failing, tell the state machine to loiter. - */ - void set_rtl_item(); - /** * Translate mission item to a position setpoint. */ @@ -329,7 +274,6 @@ Navigator *g_navigator; Navigator::Navigator() : /* state machine transition table */ - StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), @@ -368,11 +312,6 @@ Navigator::Navigator() : _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - - /* Initialize state machine */ - myState = NAV_STATE_NONE_ON_GROUND; - - start_none_on_ground(); } Navigator::~Navigator() @@ -615,41 +554,6 @@ Navigator::task_main() /* vehicle status updated */ if (fds[6].revents & POLLIN) { vehicle_status_update(); - - /* commander requested new navigation mode, forward it to state machine */ - switch (_vstatus.set_nav_state) { - case NAVIGATION_STATE_NONE: - dispatch(EVENT_NONE_REQUESTED); - break; - - case NAVIGATION_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; - - case NAVIGATION_STATE_MISSION: - dispatch(EVENT_MISSION_REQUESTED); - break; - - case NAVIGATION_STATE_RTL: - dispatch(EVENT_RTL_REQUESTED); - break; - - case NAVIGATION_STATE_LAND: - /* TODO: add and test this */ - // dispatch(EVENT_LAND_REQUESTED); - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - /* commander sets in-air/on-ground flag as well */ - if (_vstatus.condition_landed) { - dispatch(EVENT_LANDED); - } else { - dispatch(EVENT_TAKEN_OFF); - } } /* parameters updated */ @@ -666,30 +570,21 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(); - /* TODO: check if mission really changed */ - dispatch(EVENT_MISSION_CHANGED); } /* onboard mission updated */ if (fds[5].revents & POLLIN) { onboard_mission_update(); - /* TODO: check if mission really changed */ - dispatch(EVENT_MISSION_CHANGED); } /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); - /* TODO check if home position really changed */ - dispatch(EVENT_HOME_POSITION_CHANGED); } /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); - if (check_mission_item_reached()) { - dispatch(EVENT_MISSION_ITEM_REACHED); - } /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { @@ -765,125 +660,8 @@ Navigator::status() } else { warnx("Geofence not set"); } - - switch (myState) { - case NAV_STATE_NONE_ON_GROUND: - warnx("State: None on ground"); - break; - - case NAV_STATE_NONE_IN_AIR: - warnx("State: None in air"); - break; - - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - - case NAV_STATE_LAND: - warnx("State: Land"); - break; - - default: - warnx("State: Unknown"); - break; - } } - -StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* NAV_STATE_NONE_ON_GROUND */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - }, - { - /* NAV_STATE_NONE_IN_AIR */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_LANDED */ {ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - }, - { - /* NAV_STATE_AUTO_ON_GROUND */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - }, - { - /* NAV_STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LOITER}, - }, - { - /* NAV_STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_mission), NAV_STATE_MISSION}, - }, - { - /* NAV_STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_rtl), NAV_STATE_RTL}, - }, - { - /* NAV_STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_LANDED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LAND}, - }, -}; - +#if 0 bool Navigator::start_none_on_ground() { @@ -984,13 +762,11 @@ Navigator::set_mission_items() /* if we fail to set the current mission, continue to loiter */ if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) { - dispatch(EVENT_LOITER_REQUESTED); return false; } /* if we got an RTL mission item, switch to RTL mode and give up */ if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - dispatch(EVENT_RTL_REQUESTED); return false; } @@ -1042,7 +818,6 @@ Navigator::start_rtl() } /* if RTL doesn't work, fallback to loiter */ - dispatch(EVENT_LOITER_REQUESTED); return false; } @@ -1066,7 +841,6 @@ Navigator::advance_rtl() return true; } - dispatch(EVENT_LOITER_REQUESTED); return false; } @@ -1105,7 +879,7 @@ Navigator::start_land() _update_triplet = true; return true; } - +#endif void Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { @@ -1147,7 +921,7 @@ Navigator::mission_item_to_position_setpoint(const mission_item_s *item, positio sp->type = SETPOINT_TYPE_NORMAL; } } - +#if 0 bool Navigator::check_mission_item_reached() { @@ -1251,12 +1025,12 @@ Navigator::reset_reached() _waypoint_yaw_reached = false; } - +#endif void Navigator::publish_position_setpoint_triplet() { /* update navigation state */ - _pos_sp_triplet.nav_state = static_cast(myState); + /* TODO: set nav_state */ /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index 38378651b..e6011fdef 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -33,7 +33,7 @@ /** * @file state_table.h - * + * * Finite-State-Machine helper class for state table * @author: Julian Oes */ @@ -44,32 +44,32 @@ class StateTable { public: - typedef bool (StateTable::*Action)(); + typedef void (StateTable::*Action)(); struct Tran { Action action; unsigned nextState; }; - + StateTable(Tran const *table, unsigned nStates, unsigned nSignals) : myTable(table), myNsignals(nSignals), myNstates(nStates) {} - + #define NO_ACTION &StateTable::doNothing #define ACTION(_target) StateTable::Action(_target) virtual ~StateTable() {} - + void dispatch(unsigned const sig) { /* get transition using state table */ Tran const *t = myTable + myState*myNsignals + sig; - /* first up change state, this allows to do further dispatchs in the state functions */ - - /* now execute state function, if it runs with success, accept new state */ - if ((this->*(t->action))()) { - myState = t->nextState; - } + + /* accept new state */ + myState = t->nextState; + + /* */ + (this->*(t->action))(); } - bool doNothing() { - return true; + void doNothing() { + return; } protected: unsigned myState; @@ -79,4 +79,4 @@ private: unsigned myNstates; }; -#endif \ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 7c3921198..ad654a9ff 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -56,6 +56,7 @@ struct mission_result_s bool mission_reached; /**< true if mission has been reached */ unsigned mission_index_reached; /**< index of the mission which has been reached */ unsigned index_current_mission; /**< index of the current mission */ + bool mission_finished; /**< true if mission has been completed */ }; /** diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 85e8ef8a5..c2741a05b 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -74,14 +74,13 @@ struct position_setpoint_s }; typedef enum { - NAV_STATE_NONE_ON_GROUND = 0, - NAV_STATE_NONE_IN_AIR, - NAV_STATE_AUTO_ON_GROUND, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX + NAV_STATE_MANUAL = 0, + NAV_STATE_POSCTL, + NAV_STATE_AUTO, + NAV_STATE_RC_LOSS, + NAV_STATE_DL_LOSS, + NAV_STATE_TERMINATION, + MAX_NAV_STATE } nav_state_t; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index d902dc49e..259d7329e 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,9 +63,11 @@ typedef enum { MAIN_STATE_MANUAL = 0, MAIN_STATE_ALTCTL, MAIN_STATE_POSCTL, - MAIN_STATE_AUTO, + MAIN_STATE_AUTO_MISSION, + MAIN_STATE_AUTO_LOITER, + MAIN_STATE_AUTO_RTL, MAIN_STATE_ACRO, - MAIN_STATE_MAX + MAIN_STATE_MAX, } main_state_t; // If you change the order, add or remove arming_state_t states make sure to update the arrays @@ -78,7 +80,7 @@ typedef enum { ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, ARMING_STATE_IN_AIR_RESTORE, - ARMING_STATE_MAX + ARMING_STATE_MAX, } arming_state_t; typedef enum { @@ -88,18 +90,25 @@ typedef enum { typedef enum { FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RTL, /**< Return To Launch */ - FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_RTL_RC, /**< Return To Launch on remote control loss */ + FAILSAFE_STATE_RTL_DL, /**< Return To Launch on datalink loss */ + FAILSAFE_STATE_LAND, /**< Land as safe as possible */ FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ - FAILSAFE_STATE_MAX + FAILSAFE_STATE_MAX, } failsafe_state_t; typedef enum { - NAVIGATION_STATE_NONE = 0, - NAVIGATION_STATE_MISSION, - NAVIGATION_STATE_LOITER, - NAVIGATION_STATE_RTL, - NAVIGATION_STATE_LAND + NAVIGATION_STATE_MANUAL = 0, + NAVIGATION_STATE_ACRO, + NAVIGATION_STATE_ALTCTL, + NAVIGATION_STATE_POSCTL, + NAVIGATION_STATE_AUTO_MISSION, + NAVIGATION_STATE_AUTO_LOITER, + NAVIGATION_STATE_AUTO_RTL, + NAVIGATION_STATE_AUTO_RTL_RC, + NAVIGATION_STATE_AUTO_RTL_DL, + NAVIGATION_STATE_LAND, + NAVIGATION_STATE_TERMINATION, } navigation_state_t; enum VEHICLE_MODE_FLAG { @@ -160,10 +169,10 @@ struct vehicle_status_s { uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - main_state_t main_state; /**< main state machine */ + main_state_t main_state; /**< main state machine */ navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ - hil_state_t hil_state; /**< current hil state */ + hil_state_t hil_state; /**< current hil state */ failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ -- cgit v1.2.3 From c8903b167230bc2efdb908e78135a0fa0abc745c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 27 May 2014 23:24:01 +0200 Subject: commander: modes and RC loss working now --- src/modules/commander/commander.cpp | 69 ++++++++++++++++++++----------------- 1 file changed, 37 insertions(+), 32 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 908384bb6..5638bc09f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -625,8 +625,10 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[0] = "MANUAL"; main_states_str[1] = "ALTCTL"; main_states_str[2] = "POSCTL"; - main_states_str[3] = "AUTO"; - main_states_str[4] = "ACRO"; + main_states_str[3] = "AUTO_MISSION"; + main_states_str[4] = "AUTO_LOITER"; + main_states_str[5] = "AUTO_RTL"; + main_states_str[6] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[0] = "INIT"; @@ -639,9 +641,10 @@ int commander_thread_main(int argc, char *argv[]) char *failsafe_states_str[FAILSAFE_STATE_MAX]; failsafe_states_str[0] = "NORMAL"; - failsafe_states_str[1] = "RTL"; - failsafe_states_str[2] = "LAND"; - failsafe_states_str[3] = "TERMINATION"; + failsafe_states_str[1] = "RTL_RC"; + failsafe_states_str[2] = "RTL_DL"; + failsafe_states_str[3] = "LAND"; + failsafe_states_str[4] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -1151,12 +1154,9 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - warnx("arming status1: %d", status.arming_state); arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - warnx("arming status2: %d", status.arming_state); if (arming_ret == TRANSITION_CHANGED) { - warnx("changed"); arming_state_changed = true; } @@ -1198,6 +1198,9 @@ int commander_thread_main(int argc, char *argv[]) if (status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; + + status.failsafe_state = FAILSAFE_STATE_NORMAL; + failsafe_state_changed = true; } } @@ -1215,7 +1218,6 @@ int commander_thread_main(int argc, char *argv[]) arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed); if (arming_ret == TRANSITION_CHANGED) { - warnx("stick 1"); arming_state_changed = true; } stick_off_counter = 0; @@ -1241,7 +1243,6 @@ int commander_thread_main(int argc, char *argv[]) } else { arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); if (arming_ret == TRANSITION_CHANGED) { - warnx("stick 2"); arming_state_changed = true; } } @@ -1294,29 +1295,19 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - } - } - if (armed.armed) { - /* if RC is lost, switch to RTL_RC or Termination unless a mission is running - * and not yet finished) */ - if (status.rc_signal_lost - && !(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; - } else { - status.failsafe_state = FAILSAFE_STATE_LAND; + if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { + + /* if we have a global position, we can switch to RTL, if not, we can try to land */ + if (status.condition_global_position_valid) { + status.failsafe_state = FAILSAFE_STATE_RTL_RC; + } else { + status.failsafe_state = FAILSAFE_STATE_LAND; + } + failsafe_state_changed = true; } } - - /* if the data link is lost, switch to RTL_DL or Termination */ - /* TODO: add this */ - - } else { - /* reset failsafe when disarmed */ - status.failsafe_state = FAILSAFE_STATE_NORMAL; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1613,10 +1604,24 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_ON: // AUTO - res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + if (sp_man->return_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_AUTO_RTL); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + } else if (sp_man->loiter_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + } else { + res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } // else fallback to ALTCTL (POSCTL likely will not work too) -- cgit v1.2.3 From 3d9dd86900fcdc146b501efda2d2bcb0d51b3621 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 May 2014 00:12:59 +0200 Subject: mavlink mission item takeoff: read correct param for minimal pitch --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 28dd97fca..bb1ad86ef 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -415,7 +415,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) { - + Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { @@ -886,7 +886,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item switch (mavlink_mission_item->command) { case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param2; + mission_item->pitch_min = mavlink_mission_item->param1; break; default: -- cgit v1.2.3 From 36c938a187e63fa5ec6d72b9ac5628334b23b837 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 May 2014 00:14:28 +0200 Subject: mtecs: support overriding limit parameters --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 55 +++++++++++++++--- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 67 +++++++++++++++++++++- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 3 +- 3 files changed, 113 insertions(+), 12 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 087e1b476..d370bf906 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -79,7 +79,8 @@ mTecs::~mTecs() { } -int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode) +int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!isfinite(flightPathAngle) || !isfinite(altitude) || @@ -105,10 +106,12 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* use flightpath angle setpoint for total energy control */ - return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode); + return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, + airspeedSp, mode, limitOverride); } -int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode) +int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || @@ -135,10 +138,12 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* use longitudinal acceleration setpoint for total energy control */ - return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode); + return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, + accelerationLongitudinalSp, mode, limitOverride); } -int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode) +int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, + float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || @@ -180,8 +185,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); } - /* Check airspeed: if below safe value switch to underspeed mode */ - if (airspeed < _airspeedMin.get()) { + /* Check airspeed: if below safe value switch to underspeed mode (if not in takeoff mode) */ + if (!TECS_MODE_LAND && airspeed < _airspeedMin.get()) { mode = TECS_MODE_UNDERSPEED; } @@ -202,6 +207,16 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; } + /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by + * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector + * is running) */ + bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ? + _controlTotalEnergy.getOutputLimiter() : + *outputLimiterThrottle, + outputLimiterPitch == NULL ? + _controlEnergyDistribution.getOutputLimiter() : + *outputLimiterPitch); + /* Write part of the status message */ _status.airspeedDerivativeSp = airspeedDerivativeSp; _status.airspeedDerivative = airspeedDerivative; @@ -280,5 +295,29 @@ void mTecs::debug(const char *fmt, ...) { debug_print(fmt, args); } -} /* namespace fwPosctrl */ +bool mTecs::LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle, + BlockOutputLimiter &outputLimiterPitch) +{ + bool ret = false; + if (overrideThrottleMinEnabled) { + outputLimiterThrottle.setMin(overrideThrottleMin); + ret = true; + } + if (overrideThrottleMaxEnabled) { + outputLimiterThrottle.setMax(overrideThrottleMax); + ret = true; + } + if (overridePitchMinEnabled) { + outputLimiterPitch.setMin(overridePitchMin); + ret = true; + } + if (overridePitchMaxEnabled) { + outputLimiterPitch.setMax(overridePitchMax); + ret = true; + } + + return ret; +} + +} /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 376d39698..1a787df72 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -68,20 +68,81 @@ public: TECS_MODE_LAND_THROTTLELIM } tecs_mode; + + /* A small class which provides helper fucntions to override control output limits which are usually set by + * parameters in special cases + */ + class LimitOverride + { + public: + LimitOverride() : + overrideThrottleMinEnabled(false), + overrideThrottleMaxEnabled(false), + overridePitchMinEnabled(false), + overridePitchMaxEnabled(false) + {}; + + ~LimitOverride() {}; + + /* + * Override the limits of the outputlimiter instances given by the arguments with the limits saved in + * this class (if enabled) + * @return true if the limit was applied + */ + bool applyOverride(BlockOutputLimiter &outputLimiterThrottle, + BlockOutputLimiter &outputLimiterPitch); + + /* Functions to enable or disable the override */ + void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled, + &overrideThrottleMin, value); } + void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); } + void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled, + &overrideThrottleMax, value); } + void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); } + void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled, + &overridePitchMin, value); } + void disablePitchMinOverride() { disable(&overridePitchMinEnabled); } + void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled, + &overridePitchMax, value); } + void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); } + + protected: + bool overrideThrottleMinEnabled; + float overrideThrottleMin; + bool overrideThrottleMaxEnabled; + float overrideThrottleMax; + bool overridePitchMinEnabled; + float overridePitchMin; //in degrees (replaces param values) + bool overridePitchMaxEnabled; + float overridePitchMax; //in degrees (replaces param values) + + /* Enable a specific limit override */ + void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; + warnx("value %.3f", value); + }; + /* Disable a specific limit override */ + void disable(bool *flag) { *flag = false; }; + + + }; + /* * Control in altitude setpoint and speed mode */ - int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode); + int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode */ - int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode); + int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) */ - int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode); + int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, + float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride); /* * Reset all integrators diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index a7acd95de..e4e405227 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -89,6 +89,8 @@ public: bool isAngularLimit() {return _isAngularLimit ;} float getMin() { return _min.get(); } float getMax() { return _max.get(); } + void setMin(float value) { _min.set(value); } + void setMax(float value) { _max.set(value); } protected: //attributes bool _isAngularLimit; @@ -96,7 +98,6 @@ protected: control::BlockParamFloat _max; }; -typedef /* A combination of feed forward, P and I gain using the output limiter*/ class BlockFFPILimited: public SuperBlock -- cgit v1.2.3 From bfb27ff7bd27e3c89eb0eebc923c9ab7bd7c04fe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 May 2014 00:14:56 +0200 Subject: fw pos control: set takeoff min pitch for mtecs --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e3b6eb261..e43469b70 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1452,13 +1452,22 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ fwPosctrl::mTecs::tecs_mode mode) { if (_mTecs.getEnabled()) { + /* Using mtecs library: prepare arguments for mtecs call */ float flightPathAngle = 0.0f; float ground_speed_length = ground_speed.length(); if (ground_speed_length > FLT_EPSILON) { flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode); + fwPosctrl::mTecs::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); } else { + /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, climbout_mode, climbout_pitch_min_rad, -- cgit v1.2.3 From 6df2fe9aebb008932055b85f134908127d8b3931 Mon Sep 17 00:00:00 2001 From: Kynos Date: Thu, 29 May 2014 21:23:57 +0200 Subject: U-blox driver rework, step 1 Handle u-blox 7+8 GNSS, too. Disabled NAV-SVINFO for now (will be rewritten as an optional feature in a later step). Reduced parser buffer size from 300 to 80. --- src/drivers/gps/ubx.cpp | 37 ++++++++++++++++++++++++--------- src/drivers/gps/ubx.h | 55 ++++++++++++++++++++++++++++++++++++++++++------- 2 files changed, 74 insertions(+), 18 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 19cf5beec..ee53e2ec4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -219,12 +219,14 @@ UBX::configure(unsigned &baudrate) return 1; } +#ifdef UBX_ENABLE_NAV_SVINFO configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("MSG CFG FAIL: NAV SVINFO"); return 1; } +#endif configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); @@ -386,7 +388,6 @@ UBX::parse_char(uint8_t b) if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes /* compare checksum */ if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { - decode_init(); return 1; // message received successfully } else { @@ -399,7 +400,7 @@ UBX::parse_char(uint8_t b) _rx_count++; } else { - warnx("buffer full"); + warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); decode_init(); return -1; } @@ -450,6 +451,7 @@ UBX::handle_message() _gps_position->s_variance_m_s = packet->sAcc; _gps_position->p_variance_m = packet->pAcc; _gps_position->timestamp_variance = hrt_absolute_time(); + _gps_position->satellites_visible = packet->numSV; ret = 1; break; @@ -486,6 +488,7 @@ UBX::handle_message() break; } +#ifdef UBX_ENABLE_NAV_SVINFO case UBX_MESSAGE_NAV_SVINFO: { //printf("GOT NAV_SVINFO\n"); const int length_part1 = 8; @@ -527,7 +530,9 @@ UBX::handle_message() _gps_position->satellite_azimuth[i] = 0; } - _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + /* Note: _gps_position->satellites_visible is taken from NAV-SOL now. */ + /* _gps_position->satellites_visible = satellites_used; */ // visible ~= used but we are interested in the used ones + /* TODO: satellites_used may be written into a new field after sat monitoring data got separated from _gps_position */ if (packet_part1->numCh > 0) { _gps_position->satellite_info_available = true; @@ -541,6 +546,7 @@ UBX::handle_message() ret = 1; break; } +#endif /* #ifdef UBX_ENABLE_NAV_SVINFO */ case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -573,23 +579,34 @@ UBX::handle_message() break; } - case UBX_CLASS_MON: { + case UBX_CLASS_MON: switch (_message_id) { - case UBX_MESSAGE_MON_HW: { - struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; + case UBX_MESSAGE_MON_HW: + switch (_payload_size) { - _gps_position->noise_per_ms = p->noisePerMS; - _gps_position->jamming_indicator = p->jamInd; + case UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE: /* u-blox 6 msg format */ + _gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->noisePerMS; + _gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->jamInd; + ret = 1; + break; + case UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE: /* u-blox 7+ msg format */ + _gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->noisePerMS; + _gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->jamInd; ret = 1; break; + + default: // unexpected payload size: + ret = 1; // ignore but don't disable msg + break; } + break; default: break; } - } + break; default: break; @@ -597,7 +614,7 @@ UBX::handle_message() if (ret == 0) { /* message not handled */ - warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); + warnx("ubx: message not handled: 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); hrt_abstime t = hrt_absolute_time(); diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5cf47b60b..56f73fa63 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -48,6 +48,10 @@ #include "gps_helper.h" +/* TODO: processing of NAV_SVINFO disabled, has to be re-written as an optional feature */ +/* TODO: make this a command line option, allocate buffer dynamically */ +#undef UBX_ENABLE_NAV_SVINFO + #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 @@ -60,10 +64,8 @@ /* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 -//#define UBX_MESSAGE_NAV_DOP 0x04 #define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_VELNED 0x12 -//#define UBX_MESSAGE_RXM_SVSI 0x20 #define UBX_MESSAGE_NAV_TIMEUTC 0x21 #define UBX_MESSAGE_NAV_SVINFO 0x30 #define UBX_MESSAGE_ACK_NAK 0x00 @@ -72,9 +74,22 @@ #define UBX_MESSAGE_CFG_MSG 0x01 #define UBX_MESSAGE_CFG_RATE 0x08 #define UBX_MESSAGE_CFG_NAV5 0x24 - #define UBX_MESSAGE_MON_HW 0x09 +/* Rx msg payload sizes */ +#define UBX_NAV_POSLLH_RX_PAYLOAD_SIZE 28 /**< NAV_POSLLH Rx msg payload size */ +#define UBX_NAV_SOL_RX_PAYLOAD_SIZE 52 /**< NAV_SOL Rx msg payload size */ +#define UBX_NAV_VELNED_RX_PAYLOAD_SIZE 36 /**< NAV_VELNED Rx msg payload size */ +#define UBX_NAV_TIMEUTC_RX_PAYLOAD_SIZE 20 /**< NAV_TIMEUTC Rx msg payload size */ +#define UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE 68 /**< MON_HW Rx msg payload size for u-blox 6 and below */ +#define UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE 60 /**< MON_HW Rx msg payload size for u-blox 7 and above */ +#define UBX_MAX_RX_PAYLOAD_SIZE 70 /**< arbitrary maximum for calculating parser buffer size w/o NAV_SVINFO active */ + +/* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */ +#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */ +#define UBX_NAV_SVINFO_RX_PAYLOAD_SIZE (8 + 12 * UBX_MAX_NUM_SAT) /**< NAV_SVINFO Rx msg payload size */ + +/* CFG class Tx msg defs */ #define UBX_CFG_PRT_LENGTH 20 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ @@ -87,7 +102,6 @@ #define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */ #define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */ - #define UBX_CFG_NAV5_LENGTH 36 #define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005 /**< XXX only update dynamic model and fix mode */ #define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ @@ -98,7 +112,6 @@ #define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10 -#define UBX_MAX_PAYLOAD_LENGTH 500 // ************ /** the structures of the binary packets */ @@ -140,7 +153,7 @@ typedef struct { uint32_t sAcc; uint16_t pDOP; uint8_t reserved1; - uint8_t numSV; + uint8_t numSV; /**< Number of SVs used in Nav Solution */ uint32_t reserved2; uint8_t ck_a; uint8_t ck_b; @@ -213,7 +226,7 @@ typedef struct { uint8_t ck_b; } gps_bin_nav_velned_packet_t; -struct gps_bin_mon_hw_packet { +struct gps_bin_mon_hw_ubx6_packet { uint32_t pinSel; uint32_t pinBank; uint32_t pinDir; @@ -233,6 +246,25 @@ struct gps_bin_mon_hw_packet { uint32_t pullL; }; +struct gps_bin_mon_hw_ubx7_packet { + uint32_t pinSel; + uint32_t pinBank; + uint32_t pinDir; + uint32_t pinVal; + uint16_t noisePerMS; + uint16_t agcCnt; + uint8_t aStatus; + uint8_t aPower; + uint8_t flags; + uint8_t __reserved1; + uint32_t usedMask; + uint8_t VP[17]; + uint8_t jamInd; + uint16_t __reserved3; + uint32_t pinIrq; + uint32_t pulLH; + uint32_t pullL; +}; //typedef struct { // int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ @@ -343,7 +375,14 @@ typedef enum { //typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; #pragma pack(pop) -#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer +/* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */ +/* TODO: make this a command line option, allocate buffer dynamically */ +#define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */ +#ifdef UBX_ENABLE_NAV_SVINFO + #define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) +#else + #define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) +#endif class UBX : public GPS_Helper { -- cgit v1.2.3 From 7349fa90376714b00fe920538950b31a46169d60 Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 30 May 2014 11:03:16 +0200 Subject: Beautifications for tab size 8 Beautifications for tab size 8 --- src/drivers/gps/ubx.cpp | 6 ++-- src/drivers/gps/ubx.h | 74 ++++++++++++++++++++++++------------------------- 2 files changed, 40 insertions(+), 40 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index ee53e2ec4..d7e9454ae 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -59,10 +59,10 @@ #include "ubx.h" -#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK -#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received +#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK +#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received #define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls -#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval +#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _fd(fd), diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 56f73fa63..b844c315e 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -34,7 +34,7 @@ /** * @file ubx.h * - * U-Blox protocol definition. Following u-blox 6/7 Receiver Description + * U-Blox protocol definition. Following u-blox 6/7/8 Receiver Description * including Prototol Specification. * * @author Thomas Gubler @@ -86,7 +86,7 @@ #define UBX_MAX_RX_PAYLOAD_SIZE 70 /**< arbitrary maximum for calculating parser buffer size w/o NAV_SVINFO active */ /* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */ -#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */ +#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */ #define UBX_NAV_SVINFO_RX_PAYLOAD_SIZE (8 + 12 * UBX_MAX_NUM_SAT) /**< NAV_SVINFO Rx msg payload size */ /* CFG class Tx msg defs */ @@ -127,21 +127,21 @@ struct ubx_header { typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - int32_t lon; /**< Longitude * 1e-7, deg */ - int32_t lat; /**< Latitude * 1e-7, deg */ - int32_t height; /**< Height above Ellipsoid, mm */ - int32_t height_msl; /**< Height above mean sea level, mm */ - uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ - uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ + int32_t lon; /**< Longitude * 1e-7, deg */ + int32_t lat; /**< Latitude * 1e-7, deg */ + int32_t height; /**< Height above Ellipsoid, mm */ + int32_t height_msl; /**< Height above mean sea level, mm */ + uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ + uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_posllh_packet_t; typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */ - int16_t week; /**< GPS week (GPS time) */ - uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ + int16_t week; /**< GPS week (GPS time) */ + uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ uint8_t flags; int32_t ecefX; int32_t ecefY; @@ -153,7 +153,7 @@ typedef struct { uint32_t sAcc; uint16_t pDOP; uint8_t reserved1; - uint8_t numSV; /**< Number of SVs used in Nav Solution */ + uint8_t numSV; /**< Number of SVs used in Nav Solution */ uint32_t reserved2; uint8_t ck_a; uint8_t ck_b; @@ -163,47 +163,47 @@ typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */ int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */ - uint16_t year; /**< Year, range 1999..2099 (UTC) */ - uint8_t month; /**< Month, range 1..12 (UTC) */ - uint8_t day; /**< Day of Month, range 1..31 (UTC) */ - uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */ - uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */ - uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */ + uint16_t year; /**< Year, range 1999..2099 (UTC) */ + uint8_t month; /**< Month, range 1..12 (UTC) */ + uint8_t day; /**< Day of Month, range 1..31 (UTC) */ + uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */ + uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */ + uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */ uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_timeutc_packet_t; //typedef struct { -// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ -// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ -// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ -// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ -// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ -// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ -// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ -// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ +// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ +// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ +// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ +// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ +// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ +// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ +// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ +// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ // uint8_t ck_a; // uint8_t ck_b; //} gps_bin_nav_dop_packet_t; typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint8_t numCh; /**< Number of channels */ + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint8_t numCh; /**< Number of channels */ uint8_t globalFlags; uint16_t reserved2; } gps_bin_nav_svinfo_part1_packet_t; typedef struct { - uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ - uint8_t svid; /**< Satellite ID */ + uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ + uint8_t svid; /**< Satellite ID */ uint8_t flags; uint8_t quality; - uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */ - int8_t elev; /**< Elevation in integer degrees */ - int16_t azim; /**< Azimuth in integer degrees */ - int32_t prRes; /**< Pseudo range residual in centimetres */ + uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */ + int8_t elev; /**< Elevation in integer degrees */ + int16_t azim; /**< Azimuth in integer degrees */ + int32_t prRes; /**< Pseudo range residual in centimetres */ } gps_bin_nav_svinfo_part2_packet_t; @@ -268,8 +268,8 @@ struct gps_bin_mon_hw_ubx7_packet { //typedef struct { // int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ -// int16_t week; /**< Measurement GPS week number */ -// uint8_t numVis; /**< Number of visible satellites */ +// int16_t week; /**< Measurement GPS week number */ +// uint8_t numVis; /**< Number of visible satellites */ // // //... rest of package is not used in this implementation // @@ -395,7 +395,7 @@ public: private: /** - * Parse the binary MTK packet + * Parse the binary UBX packet */ int parse_char(uint8_t b); -- cgit v1.2.3 From 9bad828bc0033c7017978de2321d3a8698c0afc6 Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 30 May 2014 14:30:25 +0200 Subject: U-blox driver rework, step 2 Moved satellite info from vehicle_gps_position_s into a new uORB topic satellite_info. Renamed satellites_visible to satellites_used to reflect true content. sdlog2 will log info for GPS satellites only for now. --- src/drivers/blinkm/blinkm.cpp | 8 +-- src/drivers/gps/gps.cpp | 2 +- src/drivers/gps/mtk.cpp | 2 +- src/drivers/gps/ubx.cpp | 3 +- src/drivers/hott/messages.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/sdlog2/sdlog2.c | 48 ++++++++------ src/modules/uORB/Subscription.cpp | 2 + src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/satellite_info.h | 86 ++++++++++++++++++++++++++ src/modules/uORB/topics/vehicle_gps_position.h | 9 +-- 12 files changed, 130 insertions(+), 40 deletions(-) create mode 100644 src/modules/uORB/topics/satellite_info.h diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 5c502f682..39bf1fcfc 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -559,13 +559,7 @@ BlinkM::led() } /* get number of used satellites in navigation */ - num_of_used_sats = 0; - - for(unsigned satloop=0; satloopget_position_update_rate()); diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 680f00d97..7b957e55d 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -263,7 +263,7 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->epv_m = _gps_position->eph_m; // unknown in mtk custom mode, so we cheat with eph _gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad - _gps_position->satellites_visible = packet.satellites; + _gps_position->satellites_used = packet.satellites; /* convert time and date information to unix timestamp */ struct tm timeinfo; //TODO: test this conversion diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index d7e9454ae..462875b6c 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include "ubx.h" @@ -451,7 +452,7 @@ UBX::handle_message() _gps_position->s_variance_m_s = packet->sAcc; _gps_position->p_variance_m = packet->pAcc; _gps_position->timestamp_variance = hrt_absolute_time(); - _gps_position->satellites_visible = packet->numSV; + _gps_position->satellites_used = packet->numSV; ret = 1; break; diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 1e779e8dc..086132573 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -226,7 +226,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.sensor_id = GPS_SENSOR_ID; msg.sensor_text_id = GPS_SENSOR_TEXT_ID; - msg.gps_num_sat = gps.satellites_visible; + msg.gps_num_sat = gps.satellites_used; /* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */ msg.gps_fix_char = (uint8_t)(gps.fix_type + 48); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 933478f56..146e2d4b8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -548,7 +548,7 @@ protected: cm_uint16_from_m_float(gps->epv_m), gps->vel_m_s * 100.0f, _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->satellites_used); } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 53769e0cf..46da1a0be 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -682,9 +682,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.vel_ned_valid = true; hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); - hil_gps.timestamp_satellites = timestamp; hil_gps.fix_type = gps.fix_type; - hil_gps.satellites_visible = gps.satellites_visible; + hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used? if (_gps_pub < 0) { _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 577cadfbb..4ff6c30aa 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -139,6 +140,8 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1); fds[fdsc_count].events = POLLIN; \ fdsc_count++; +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) + static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -941,6 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct estimator_status_report estimator_status; struct system_power_s system_power; struct servorail_status_s servorail_status; + struct satellite_info_s sat_info; } buf; memset(&buf, 0, sizeof(buf)); @@ -1000,6 +1004,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int triplet_sub; int gps_pos_sub; + int sat_info_sub; int vicon_pos_sub; int flow_sub; int rc_sub; @@ -1017,6 +1022,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -1053,6 +1059,9 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; + /* initialize calculated mean SNR */ + float snr_mean = 0.0f; + /* enable logging on start if needed */ if (log_on_start) { /* check GPS topic to get GPS time */ @@ -1115,14 +1124,6 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION - UNIT #1 --- */ if (gps_pos_updated) { - float snr_mean = 0.0f; - - for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) { - snr_mean += buf_gps_pos.satellite_snr[i]; - } - - snr_mean /= buf_gps_pos.satellites_visible; - log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; @@ -1135,44 +1136,55 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s; log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; - log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible; + log_msg.body.log_GPS.sats = buf_gps_pos.satellites_used; log_msg.body.log_GPS.snr_mean = snr_mean; log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms; log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator; LOGBUFFER_WRITE_AND_COUNT(GPS); + } + + /* --- SATELLITE INFO - UNIT #1 --- */ + if (_extended_logging) { + + if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) { - if (_extended_logging) { /* log the SNR of each satellite for a detailed view of signal quality */ - unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]); + unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0])); unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]); log_msg.msg_type = LOG_GS0A_MSG; memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); - /* fill set A */ - for (unsigned i = 0; i < gps_msg_max_snr; i++) { + snr_mean = 0.0f; - int satindex = buf_gps_pos.satellite_prn[i] - 1; + /* fill set A and calculate mean SNR */ + for (unsigned i = 0; i < sat_info_count; i++) { + + snr_mean += buf.sat_info.snr[i]; + + int satindex = buf.sat_info.svid[i] - 1; /* handles index exceeding and wraps to to arithmetic errors */ if ((satindex >= 0) && (satindex < (int)log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0A); + snr_mean /= sat_info_count; log_msg.msg_type = LOG_GS0B_MSG; memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); + /* fill set B */ - for (unsigned i = 0; i < gps_msg_max_snr; i++) { + for (unsigned i = 0; i < sat_info_count; i++) { /* get second bank of satellites, thus deduct bank size from index */ - int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr; + int satindex = buf.sat_info.svid[i] - 1 - log_max_snr; /* handles index exceeding and wraps to to arithmetic errors */ if ((satindex >= 0) && (satindex < (int)log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0B); diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index c1d1a938f..44b6debc7 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -40,6 +40,7 @@ #include "topics/parameter_update.h" #include "topics/actuator_controls.h" #include "topics/vehicle_gps_position.h" +#include "topics/satellite_info.h" #include "topics/sensor_combined.h" #include "topics/vehicle_attitude.h" #include "topics/vehicle_global_position.h" @@ -88,6 +89,7 @@ T Subscription::getData() { template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e..fc12b9ed5 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -75,6 +75,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s); #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); +#include "topics/satellite_info.h" +ORB_DEFINE(satellite_info, struct satellite_info_s); + #include "topics/home_position.h" ORB_DEFINE(home_position, struct home_position_s); diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h new file mode 100644 index 000000000..5f799eda4 --- /dev/null +++ b/src/modules/uORB/topics/satellite_info.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file satellite_info.h + * Definition of the GNSS satellite info uORB topic. + */ + +#ifndef TOPIC_SAT_INFO_H_ +#define TOPIC_SAT_INFO_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * GNSS Satellite Info. + */ +struct satellite_info_s { + uint64_t timestamp; /**< Timestamp of satellite information */ + uint8_t count; /**< Number of satellites in satellite_...[] arrays */ + uint8_t svid[20]; /**< Space vehicle ID [1..255], see scheme below */ + uint8_t used[20]; /**< 0: Satellite not used, 1: used for navigation */ + uint8_t elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ +}; + +/** + * NAV_SVINFO space vehicle ID (svid) scheme according to u-blox protocol specs + * u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_(UBX-13003221).pdf + * + * GPS 1-32 + * SBAS 120-158 + * Galileo 211-246 + * BeiDou 159-163, 33-64 + * QZSS 193-197 + * GLONASS 65-96, 255 + * + */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(satellite_info); + +#endif diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 5924a324d..a50f02139 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -82,14 +82,7 @@ struct vehicle_gps_position_s { uint64_t timestamp_time; /**< Timestamp for time information */ uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ - uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ - uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ - uint8_t satellite_prn[20]; /**< Global satellite ID */ - uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ - uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t satellite_snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ - bool satellite_info_available; /**< 0 for no info, 1 for info available */ + uint8_t satellites_used; /**< Number of satellites used */ }; /** -- cgit v1.2.3 From 33356ed50386ceb086595e5bd13e42d8fd924add Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 19:36:06 +0200 Subject: mtecs publish state --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++---- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 + src/modules/fw_pos_control_l1/mtecs/mTecs.h | 9 --------- src/modules/uORB/topics/tecs_status.h | 10 ++++++++++ 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e43469b70..7fd7ed07f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -386,7 +386,7 @@ private: bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL); + tecs_mode mode = TECS_MODE_NORMAL); }; @@ -1018,7 +1018,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 0.0f, throttle_max, throttle_land, false, flare_pitch_angle_rad, _pos_sp_triplet.current.alt + relative_alt, ground_speed, - land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND); + land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -1111,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(10.0f)), _global_pos.alt, ground_speed, - fwPosctrl::mTecs::TECS_MODE_TAKEOFF); + TECS_MODE_TAKEOFF); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); @@ -1449,7 +1449,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - fwPosctrl::mTecs::tecs_mode mode) + tecs_mode mode) { if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index d370bf906..039fc34a8 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -224,6 +224,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight _status.totalEnergyRate = totalEnergyRate; _status.energyDistributionRateSp = energyDistributionRateSp; _status.energyDistributionRate = energyDistributionRate; + _status.mode = mode; /** update control blocks **/ /* update total energy rate control block */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 1a787df72..b7d4af0f9 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -60,15 +60,6 @@ public: mTecs(); virtual ~mTecs(); - typedef enum { - TECS_MODE_NORMAL, - TECS_MODE_UNDERSPEED, - TECS_MODE_TAKEOFF, - TECS_MODE_LAND, - TECS_MODE_LAND_THROTTLELIM - } tecs_mode; - - /* A small class which provides helper fucntions to override control output limits which are usually set by * parameters in special cases */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index f3d33ec20..fc530b295 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -50,6 +50,14 @@ * @{ */ +typedef enum { + TECS_MODE_NORMAL, + TECS_MODE_UNDERSPEED, + TECS_MODE_TAKEOFF, + TECS_MODE_LAND, + TECS_MODE_LAND_THROTTLELIM +} tecs_mode; + /** * Internal values of the (m)TECS fixed wing speed alnd altitude control system */ @@ -69,6 +77,8 @@ struct tecs_status_s { float totalEnergyRate; float energyDistributionRateSp; float energyDistributionRate; + + tecs_mode mode; }; /** -- cgit v1.2.3 From ca6e309ba05c6793543c55d9507971df6f0721c2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 19:43:58 +0200 Subject: sdlog2: log mtecs mode --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e1f3490a9..17461f587 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1508,6 +1508,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate; log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp; log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate; + log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; LOGBUFFER_WRITE_AND_COUNT(TECS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 8a0a0ad45..83832580c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -362,6 +362,8 @@ struct log_TECS_s { float totalEnergyRate; float energyDistributionRateSp; float energyDistributionRate; + + uint8_t mode; }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -419,7 +421,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "ffffffffffff", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR"), + LOG_FORMAT(TECS, "ffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From 7716a1816db9dc469f4c741442d73721469693a6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 09:13:59 +0200 Subject: mtecs: remove warnx --- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index b7d4af0f9..4643145c1 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -109,7 +109,6 @@ public: /* Enable a specific limit override */ void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; - warnx("value %.3f", value); }; /* Disable a specific limit override */ void disable(bool *flag) { *flag = false; }; -- cgit v1.2.3 From db925db460fc24ae4d02b87e6fc9cebf2b100b7a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 09:38:14 +0200 Subject: mtecs: do not calculate derivative in first iteration --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 1 + src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 6 ++++++ src/modules/fw_pos_control_l1/mtecs/mTecs.h | 4 ++++ 3 files changed, 11 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7fd7ed07f..acd623c30 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -861,6 +861,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); } } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 039fc34a8..a5e680f02 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -261,6 +261,12 @@ void mTecs::resetIntegrators() _firstIterationAfterReset = true; } +void mTecs::resetDerivatives(float airspeed) +{ + _airspeedDerivative.setU(airspeed); +} + + void mTecs::updateTimeMeasurement() { if (!_dtCalculated) { diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 4643145c1..ddd6583e8 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -139,6 +139,10 @@ public: */ void resetIntegrators(); + /* + * Reset all derivative calculations + */ + void resetDerivatives(float airspeed); /* Accessors */ bool getEnabled() {return _mTecsEnabled.get() > 0;} -- cgit v1.2.3 From bb87cb8c612bb733f0d37226c2157eb080aaf07a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 09:53:04 +0200 Subject: fw pos control: don't call tecs 50hz update loop if mtecs is enabled --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index acd623c30..954431fa3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -845,7 +845,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + if (!_mTecs.getEnabled()) { + _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + } + float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ -- cgit v1.2.3 From c4cf07b1a943b0c4c1ebb553c0322695128c396f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 10:55:50 +0200 Subject: fix stupid error in underspeed detection --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index a5e680f02..3de51695e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -186,7 +186,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight } /* Check airspeed: if below safe value switch to underspeed mode (if not in takeoff mode) */ - if (!TECS_MODE_LAND && airspeed < _airspeedMin.get()) { + if (mode != TECS_MODE_LAND && airspeed < _airspeedMin.get()) { mode = TECS_MODE_UNDERSPEED; } -- cgit v1.2.3 From 436cefd88c797992ddd066c13ec3a32ea01f1dcf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 12:13:48 +0200 Subject: make launch detector more flash efficient --- makefiles/config_px4fmu-v1_default.mk | 6 +----- src/lib/launchdetection/module.mk | 2 ++ 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 810e008ae..e4bc6fecf 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -52,7 +52,6 @@ MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top -MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/dumpfile @@ -67,12 +66,11 @@ MODULES += modules/mavlink MODULES += modules/gpio_led # -# Estimation modules (EKF/ SO3 / other filters) +# Estimation modules (EKF / other filters) # MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav -#MODULES += examples/flow_position_estimator # # Vehicle Control @@ -81,8 +79,6 @@ MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/mc_att_control MODULES += modules/mc_pos_control -#MODULES += examples/flow_position_control -#MODULES += examples/flow_speed_control # # Logging diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk index de0174e37..d92f0bb45 100644 --- a/src/lib/launchdetection/module.mk +++ b/src/lib/launchdetection/module.mk @@ -38,3 +38,5 @@ SRCS = LaunchDetector.cpp \ CatapultLaunchMethod.cpp \ launchdetection_params.c + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From ff4aec6588d6452812131b9275069ac2933543ff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 15:34:27 +0200 Subject: wind estimate: added uORB topic --- src/modules/uORB/objects_common.cpp | 2 + src/modules/uORB/topics/wind_estimate.h | 68 +++++++++++++++++++++++++++++++++ 2 files changed, 70 insertions(+) create mode 100644 src/modules/uORB/topics/wind_estimate.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index bae46e14d..7c3bb0009 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -203,3 +203,5 @@ ORB_DEFINE(estimator_status, struct estimator_status_report); #include "topics/tecs_status.h" ORB_DEFINE(tecs_status, struct tecs_status_s); +#include "topics/wind_estimate.h" +ORB_DEFINE(wind_estimate, struct wind_estimate_s); diff --git a/src/modules/uORB/topics/wind_estimate.h b/src/modules/uORB/topics/wind_estimate.h new file mode 100644 index 000000000..58333a64f --- /dev/null +++ b/src/modules/uORB/topics/wind_estimate.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file wind_estimate.h + * + * Wind estimate topic topic + * + */ + +#ifndef TOPIC_WIND_ESTIMATE_H +#define TOPIC_WIND_ESTIMATE_H + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** Wind estimate */ +struct wind_estimate_s { + + uint64_t timestamp; /**< Microseconds since system boot */ + float windspeed_north; /**< Wind component in north / X direction */ + float windspeed_east; /**< Wind component in east / Y direction */ + float covariance_north; /**< Uncertainty - set to zero (no uncertainty) if not estimated */ + float covariance_east; /**< Uncertainty - set to zero (no uncertainty) if not estimated */ +}; + +/** + * @} + */ + +ORB_DECLARE(wind_estimate); + +#endif \ No newline at end of file -- cgit v1.2.3 From 50342f96613d6a6d428b8b65a572c93696bd720f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 15:35:01 +0200 Subject: att / pos estimator: Publishing wind estimate --- .../ekf_att_pos_estimator_main.cpp | 33 +++++++++++++++++++--- src/modules/ekf_att_pos_estimator/estimator.cpp | 4 +-- 2 files changed, 31 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 0a6d3fa8f..93ed18b8d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -158,6 +159,7 @@ private: orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _local_pos_pub; /**< position in local frame */ orb_advert_t _estimator_status_pub; /**< status of the estimator */ + orb_advert_t _wind_pub; /**< wind estimate */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct gyro_report _gyro; @@ -169,6 +171,7 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ + struct wind_estimate_s _wind; /**< Wind estimate */ struct gyro_scale _gyro_offsets; struct accel_scale _accel_offsets; @@ -312,6 +315,7 @@ FixedwingEstimator::FixedwingEstimator() : _global_pos_pub(-1), _local_pos_pub(-1), _estimator_status_pub(-1), + _wind_pub(-1), _att({}), _gyro({}), @@ -323,6 +327,7 @@ FixedwingEstimator::FixedwingEstimator() : _global_pos({}), _local_pos({}), _gps({}), + _wind({}), _gyro_offsets({}), _accel_offsets({}), @@ -1328,13 +1333,31 @@ FixedwingEstimator::task_main() /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { - /* publish the attitude setpoint */ + /* publish the global position */ orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); } else { /* advertise and publish */ _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } + + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + _wind.covariance_north = 0.0f; // XXX get form filter + _wind.covariance_east = 0.0f; + + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } + } } } @@ -1396,9 +1419,11 @@ FixedwingEstimator::print_status() printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + printf("states (accel offs) [14]: %8.4f\n", (double)_ekf->states[13]); + printf("states (wind) [15-16]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); + printf("states (earth mag) [17-19]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); + printf("states (body mag) [20-22]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); + printf("states (terrain) [23]: %8.4f\n", (double)_ekf->states[22]); printf("states: %s %s %s %s %s %s %s %s %s %s\n", (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5db1adbb3..1320b4668 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2042,10 +2042,10 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max) float ret; if (val > max) { ret = max; - ekf_debug("> max: %8.4f, val: %8.4f", max, val); + ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val); } else if (val < min) { ret = min; - ekf_debug("< min: %8.4f, val: %8.4f", min, val); + ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val); } else { ret = val; } -- cgit v1.2.3 From ef6af184aa7858a9124de13c958f02b140b0f712 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 15:35:26 +0200 Subject: sdlog2: Logging wind estimate at low rate --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 10 ++++++++++ 2 files changed, 27 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 17461f587..c19579f0f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -87,6 +87,7 @@ #include #include #include +#include #include #include @@ -943,6 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct tecs_status_s tecs_status; struct system_power_s system_power; struct servorail_status_s servorail_status; + struct wind_estimate_s wind_estimate; } buf; memset(&buf, 0, sizeof(buf)); @@ -982,6 +984,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS1A_s log_GS1A; struct log_GS1B_s log_GS1B; struct log_TECS_s log_TECS; + struct log_WIND_s log_WIND; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1016,6 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int tecs_status_sub; int system_power_sub; int servorail_status_sub; + int wind_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1044,6 +1048,9 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); + subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + /* we need to rate-limit wind, as we do not need the full update rate */ + orb_set_interval(subs.wind_sub, 90); thread_running = true; @@ -1512,6 +1519,16 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(TECS); } + /* --- WIND ESTIMATE --- */ + if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) { + log_msg.msg_type = LOG_WIND_MSG; + log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north; + log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east; + log_msg.body.log_WIND.cov_x = buf.wind_estimate.covariance_north; + log_msg.body.log_WIND.cov_y = buf.wind_estimate.covariance_east; + LOGBUFFER_WRITE_AND_COUNT(WIND); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 83832580c..a6eb7a5bd 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -366,6 +366,15 @@ struct log_TECS_s { uint8_t mode; }; +/* --- WIND - WIND ESTIMATE --- */ +#define LOG_WIND_MSG 31 +struct log_WIND_s { + float x; + float y; + float cov_x; + float cov_y; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -422,6 +431,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(TECS, "ffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), + LOG_FORMAT(WIND, "fff", "X,Y,Cov"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From 41a763a50e99880147a2e4f5a6988299f8fd6dac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 15:39:38 +0200 Subject: Fix log format specifier --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a6eb7a5bd..a874351b3 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -431,7 +431,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(TECS, "ffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), - LOG_FORMAT(WIND, "fff", "X,Y,Cov"), + LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From eb02c6ce4900a3dfe96592219852b2205c5d691d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 2 Jun 2014 18:28:18 +0200 Subject: mtecs: disable underspeed mode in takeoff mode (as the comment says) --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 3de51695e..32f9f19ca 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -185,8 +185,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); } - /* Check airspeed: if below safe value switch to underspeed mode (if not in takeoff mode) */ - if (mode != TECS_MODE_LAND && airspeed < _airspeedMin.get()) { + /* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */ + if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeed < _airspeedMin.get()) { mode = TECS_MODE_UNDERSPEED; } -- cgit v1.2.3 From 5c536ae46ec44b75fea745dd79f20b0aef2c8f16 Mon Sep 17 00:00:00 2001 From: Benjamin O'Connell-Armand Date: Mon, 2 Jun 2014 23:10:03 -0400 Subject: Added processing of postion_control offboard messages (quad_swarm) --- src/modules/mavlink/mavlink_receiver.cpp | 17 ++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 561744755..78c50c422 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -102,6 +102,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _cmd_pub(-1), _flow_pub(-1), _offboard_control_sp_pub(-1), + _local_pos_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), _vicon_position_pub(-1), @@ -411,7 +412,21 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message if (_control_mode.flag_control_offboard_enabled) { if (_control_mode.flag_control_position_enabled) { - // TODO + // TODO Use something else then quad_swarm_roll_pitch_yaw_thrust + struct vehicle_local_position_setpoint_s loc_pos_sp; + memset(&loc_pos_sp, 0, sizeof(loc_pos_sp)); + + loc_pos_sp.x = offboard_control_sp.p1; + loc_pos_sp.y = offboard_control_sp.p2; + loc_pos_sp.yaw = offboard_control_sp.p3; + loc_pos_sp.z = offboard_control_sp.p4; + + if (_local_pos_sp_pub < 0) { + _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub); + + } else { + orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp); + } } else if (_control_mode.flag_control_velocity_enabled) { // TODO diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index dc31b4c5a..a2736969f 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -137,6 +137,7 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; + orb_advert_t _local_pos_sp_pub orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; orb_advert_t _vicon_position_pub; -- cgit v1.2.3 From 1fc859b1b195b587e70e4a8f6fbeb952424d0610 Mon Sep 17 00:00:00 2001 From: Benjamin O'Connell-Armand Date: Mon, 2 Jun 2014 23:11:31 -0400 Subject: Added support for postion_control offboard messages in commander. --- src/modules/commander/commander.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 90efd5115..29a5f0b4a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1809,6 +1809,14 @@ set_control_mode() control_mode.flag_control_offboard_enabled = true; switch (sp_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = false; + break; case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; -- cgit v1.2.3 From 854bb7fe089daf8bf3d4e9f2cac1cb2b99a67ac7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Jun 2014 16:01:28 +0200 Subject: navigator: mission class added (WIP) --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/navigator/mission.cpp | 438 +++++++++++++++------ src/modules/navigator/mission.h | 164 ++++++-- src/modules/navigator/module.mk | 1 - src/modules/navigator/navigator.h | 205 ++++++++++ src/modules/navigator/navigator_main.cpp | 338 ++-------------- src/modules/navigator/rtl.cpp | 31 +- src/modules/navigator/rtl.h | 21 +- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/uORB/topics/mission.h | 12 +- .../uORB/topics/position_setpoint_triplet.h | 30 +- 12 files changed, 749 insertions(+), 497 deletions(-) create mode 100644 src/modules/navigator/navigator.h diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a53118dec..b8d94fe18 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -861,7 +861,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { + if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6b45736e9..48f91481d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -415,7 +415,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) { - + Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index adbb0cfa2..f3a86666f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -38,30 +38,42 @@ * @author Julian Oes */ +#include #include #include +#include + +#include #include #include +#include #include #include +#include "navigator.h" #include "mission.h" -Mission::Mission() : - - _offboard_dataman_id(-1), - _current_offboard_mission_index(0), - _current_onboard_mission_index(0), - _offboard_mission_item_count(0), - _onboard_mission_item_count(0), - _onboard_mission_allowed(false), - _current_mission_type(MISSION_TYPE_NONE), +Mission::Mission(Navigator *navigator) : + SuperBlock(NULL, "MIS"), + _navigator(navigator), + _first_run(true), + _param_onboard_enabled(this, "ONBOARD_EN"), + _onboard_mission_sub(-1), + _offboard_mission_sub(-1), + _onboard_mission({0}), + _offboard_mission({0}), + _mission_item({0}), _mission_result_pub(-1), - _mission_result({}) + _mission_result({0}), + _mission_type(MISSION_TYPE_NONE) { + /* load initial params */ + updateParams(); + /* set initial mission items */ + reset(); } Mission::~Mission() @@ -69,134 +81,335 @@ Mission::~Mission() } void -Mission::set_offboard_dataman_id(const int new_id) +Mission::reset() { - _offboard_dataman_id = new_id; + _first_run = true; } -void -Mission::set_offboard_mission_count(int new_count) +bool +Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { - _offboard_mission_item_count = new_count; + bool updated = false; + /* check if anything has changed, and reset mission items if needed */ + if (is_onboard_mission_updated() || is_offboard_mission_updated() || _first_run) { + set_mission_items(pos_sp_triplet); + updated = true; + _first_run = false; + } + + if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { + advance_mission(); + set_mission_items(pos_sp_triplet); + updated = true; + } + return updated; } -void -Mission::set_onboard_mission_count(int new_count) +bool +Mission::is_mission_item_reached() { - _onboard_mission_item_count = new_count; + /* TODO: count turns */ +#if 0 + if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item.loiter_radius > 0.01f) { + + return false; + } +#endif + + hrt_abstime now = hrt_absolute_time(); + + if (!_waypoint_position_reached) { + + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + float altitude_amsl = _mission_item.altitude_is_relative + ? _mission_item.altitude + _navigator->get_home_position()->alt + : _mission_item.altitude; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, + &dist_xy, &dist_z); + + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + + /* require only altitude for takeoff */ + if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } else { + if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } + } + + if (_waypoint_position_reached && !_waypoint_yaw_reached) { + + /* TODO: removed takeoff, why? */ + if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { + + /* check yaw if defined only for rotary wing except takeoff */ + float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); + + if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ + _waypoint_yaw_reached = true; + } + + } else { + _waypoint_yaw_reached = true; + } + } + + /* check if the current waypoint was reached */ + if (_waypoint_position_reached && _waypoint_yaw_reached) { + + if (_time_first_inside_orbit == 0) { + _time_first_inside_orbit = now; + + // if (_mission_item.time_inside > 0.01f) { + // mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", + // (double)_mission_item.time_inside); + // } + } + + /* check if the MAV was long enough inside the waypoint orbit */ + if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) { + return true; + } + } + return false; } void -Mission::set_onboard_mission_allowed(bool allowed) -{ - _onboard_mission_allowed = allowed; +Mission::reset_mission_item_reached() { + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_first_inside_orbit = 0; } -bool -Mission::command_current_offboard_mission_index(const int new_index) +void +Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { - if (new_index >= 0) { - _current_offboard_mission_index = (unsigned)new_index; - } else { + sp->valid = true; - /* if less WPs available, reset to first WP */ - if (_current_offboard_mission_index >= _offboard_mission_item_count) { - _current_offboard_mission_index = 0; - } + sp->lat = item->lat; + sp->lon = item->lon; + sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; + sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; + + if (item->nav_cmd == NAV_CMD_TAKEOFF) { + sp->type = SETPOINT_TYPE_TAKEOFF; + + } else if (item->nav_cmd == NAV_CMD_LAND) { + sp->type = SETPOINT_TYPE_LAND; + + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + sp->type = SETPOINT_TYPE_LOITER; + + } else { + sp->type = SETPOINT_TYPE_POSITION; } - report_current_offboard_mission_item(); } bool -Mission::command_current_onboard_mission_index(int new_index) +Mission::is_onboard_mission_updated() { - if (new_index != -1) { - _current_onboard_mission_index = (unsigned)new_index; - } else { + bool updated; + orb_check(_onboard_mission_sub, &updated); - /* if less WPs available, reset to first WP */ - if (_current_onboard_mission_index >= _onboard_mission_item_count) { - _current_onboard_mission_index = 0; - } + if (!updated) { + return false; } - // TODO: implement this for onboard missions as well - // report_current_mission_item(); + + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) { + _onboard_mission.count = 0; + _onboard_mission.current_index = 0; + } + return true; } bool -Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index) +Mission::is_offboard_mission_updated() { - *onboard = false; - *index = -1; - - /* try onboard mission first */ - if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) { - _current_mission_type = MISSION_TYPE_ONBOARD; - *onboard = true; - *index = _current_onboard_mission_index; - - return true; - } + bool updated; + orb_check(_offboard_mission_sub, &updated); + if (!updated) { + return false; } - /* otherwise fallback to offboard */ - if (_current_offboard_mission_index < _offboard_mission_item_count) { + if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) { + /* Check mission feasibility, for now do not handle the return value, + * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; - if (_offboard_dataman_id == 0) { + + if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) { - _current_mission_type = MISSION_TYPE_OFFBOARD; - *onboard = false; - *index = _current_offboard_mission_index; + missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, + (size_t)_offboard_mission.count, + _navigator->get_geofence()); + } else { + _offboard_mission.count = 0; + _offboard_mission.current_index = 0; + } + return true; +} - return true; - } + +void +Mission::advance_mission() +{ + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + _onboard_mission.current_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _offboard_mission.current_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; } +} + +void +Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + warnx("set mission items"); + + set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); - /* happens when no more mission items can be added as a next item */ - _current_mission_type = MISSION_TYPE_NONE; + if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { + /* try setting onboard mission item */ + _mission_type = MISSION_TYPE_ONBOARD; + } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { + /* try setting offboard mission item */ + _mission_type = MISSION_TYPE_OFFBOARD; + } else { + _mission_type = MISSION_TYPE_NONE; + } +} + +void +Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp, + struct position_setpoint_s *previous_pos_sp) +{ + /* reuse current setpoint as previous setpoint */ + if (current_pos_sp->valid) { + memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s)); + } +} + +bool +Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) +{ + if (_param_onboard_enabled.get() > 0 + && _onboard_mission.current_index < (int)_onboard_mission.count) { + struct mission_item_s new_mission_item; + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index, + &new_mission_item)) { + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&_mission_item, current_pos_sp); + current_pos_sp->valid = true; + + /* TODO: report this somehow */ + return true; + } + } return false; } bool -Mission::get_next_mission_item(struct mission_item_s *new_mission_item) +Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { - int next_temp_mission_index = _current_onboard_mission_index + 1; + if (_offboard_mission.current_index < (int)_offboard_mission.count) { + dm_item_t dm_current; + if (_offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + struct mission_item_s new_mission_item; + if (read_mission_item(dm_current, true, &_offboard_mission.current_index, &new_mission_item)) { + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&_mission_item, current_pos_sp); + current_pos_sp->valid = true; - /* try onboard mission first */ - if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) { + report_current_offboard_mission_item(); return true; } } + return false; +} - /* then try offboard mission */ - dm_item_t dm_current; - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - next_temp_mission_index = _current_offboard_mission_index + 1; - if (next_temp_mission_index < _offboard_mission_item_count) { - if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) { - return true; +void +Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) +{ + int next_temp_mission_index = _onboard_mission.current_index + 1; + + /* try if there is a next onboard mission */ + if (next_temp_mission_index < (int)_onboard_mission.count) { + struct mission_item_s new_mission_item; + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { + /* convert next mission item to position setpoint */ + mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); + next_pos_sp->valid = true; + return; } } - /* both failed, bail out */ - return false; + /* give up */ + next_pos_sp->valid = false; + return; +} + +void +Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) +{ + /* try if there is a next offboard mission */ + int next_temp_mission_index = _offboard_mission.current_index + 1; + if (next_temp_mission_index < (int)_offboard_mission.count) { + dm_item_t dm_current; + if (_offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + struct mission_item_s new_mission_item; + if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) { + /* convert next mission item to position setpoint */ + mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); + next_pos_sp->valid = true; + return; + } + } + /* give up */ + next_pos_sp->valid = false; + return; } bool -Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item) +Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, + struct mission_item_s *new_mission_item) { /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ for (int i=0; i<10; i++) { @@ -211,25 +424,25 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { + return false; + } - /* only raise the repeat count if this is for the current mission item - * but not for the next mission item */ - if (is_current) { - (new_mission_item->do_jump_current_count)++; + /* only raise the repeat count if this is for the current mission item + * but not for the next mission item */ + if (is_current) { + (new_mission_item->do_jump_current_count)++; - /* save repeat count */ - if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } + /* save repeat count */ + if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, + new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the dataman */ + return false; } - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; - } else { - return false; } + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; } else { /* if it's not a DO_JUMP, then we were successful */ @@ -242,32 +455,12 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio return false; } -void -Mission::move_to_next() -{ - report_mission_item_reached(); - - switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - - case MISSION_TYPE_NONE: - default: - break; - } -} - void Mission::report_mission_item_reached() { - if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + if (_mission_type == MISSION_TYPE_OFFBOARD) { _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; + _mission_result.mission_index_reached = _offboard_mission.current_index; } publish_mission_result(); } @@ -275,7 +468,7 @@ Mission::report_mission_item_reached() void Mission::report_current_offboard_mission_item() { - _mission_result.index_current_mission = _current_offboard_mission_index; + _mission_result.index_current_mission = _offboard_mission.current_index; publish_mission_result(); } @@ -294,4 +487,3 @@ Mission::publish_mission_result() /* reset reached bool */ _mission_result.mission_reached = false; } - diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 5e0e9702d..e86dd25bb 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -31,70 +31,172 @@ * ****************************************************************************/ /** - * @file navigator_mission.h + * @file mission.h * Helper class to access missions - * @author Julian Oes * - * @author Julian Oes + * @author Julian Oes */ #ifndef NAVIGATOR_MISSION_H #define NAVIGATOR_MISSION_H -#include -#include +#include + +#include +#include + #include +#include +#include +#include +#include + +#include "mission_feasibility_checker.h" + +class Navigator; -class __EXPORT Mission +class Mission : public control::SuperBlock { public: /** * Constructor */ - Mission(); + Mission(Navigator *navigator); /** * Destructor */ - ~Mission(); + virtual ~Mission(); - void set_offboard_dataman_id(const int new_id); - void set_offboard_mission_count(const int new_count); - void set_onboard_mission_count(const int new_count); - void set_onboard_mission_allowed(const bool allowed); + /** + * This function is called while the mode is inactive + */ + virtual void reset(); - bool command_current_offboard_mission_index(const int new_index); - bool command_current_onboard_mission_index(const int new_index); + /** + * This function is called while the mode is active + */ + virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); - bool get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index); - bool get_next_mission_item(struct mission_item_s *mission_item); +protected: + /** + * Check if mission item has been reached + * @return true if successfully reached + */ + bool is_mission_item_reached(); + /** + * Reset all reached flags + */ + void reset_mission_item_reached(); - void move_to_next(); + /** + * Convert a mission item to a position setpoint + */ + void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp); + + class Navigator *_navigator; + + bool _waypoint_position_reached; + bool _waypoint_yaw_reached; + hrt_abstime _time_first_inside_orbit; + + bool _first_run; private: - bool read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item); + /** + * Update onboard mission topic + * @return true if onboard mission has been updated + */ + bool is_onboard_mission_updated(); + + /** + * Update offboard mission topic + * @return true if offboard mission has been updated + */ + bool is_offboard_mission_updated(); + + /** + * Move on to next mission item or switch to loiter + */ + void advance_mission(); + + /** + * Set new mission items + */ + void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet); + + /** + * Set previous position setpoint + */ + void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp, + struct position_setpoint_s *previous_pos_sp); + + /** + * Try to set the current position setpoint from an onboard mission item + * @return true if mission item successfully set + */ + bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp); + + /** + * Try to set the current position setpoint from an offboard mission item + * @return true if mission item successfully set + */ + bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp); - void report_mission_item_reached(); - void report_current_offboard_mission_item(); + /** + * Try to set the next position setpoint from an onboard mission item + */ + void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp); - void publish_mission_result(); + /** + * Try to set the next position setpoint from an offboard mission item + */ + void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp); - int _offboard_dataman_id; - int _current_offboard_mission_index; - int _current_onboard_mission_index; - int _offboard_mission_item_count; /** number of offboard mission items available */ - int _onboard_mission_item_count; /** number of onboard mission items available */ - bool _onboard_mission_allowed; + /** + * Read a mission item from the dataman and watch out for DO_JUMPS + * @return true if successful + */ + bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, + struct mission_item_s *new_mission_item); + + /** + * Report that a mission item has been reached + */ + void report_mission_item_reached(); + + /** + * Rport the current mission item + */ + void report_current_offboard_mission_item(); + + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + + control::BlockParamFloat _param_onboard_enabled; + + int _onboard_mission_sub; + int _offboard_mission_sub; + + struct mission_s _onboard_mission; + struct mission_s _offboard_mission; + + struct mission_item_s _mission_item; + + orb_advert_t _mission_result_pub; + struct mission_result_s _mission_result; enum { MISSION_TYPE_NONE, MISSION_TYPE_ONBOARD, - MISSION_TYPE_OFFBOARD, - } _current_mission_type; + MISSION_TYPE_OFFBOARD + } _mission_type; + + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ - orb_advert_t _mission_result_pub; /**< publish mission result topic */ - mission_result_s _mission_result; /**< mission result for commander/mavlink */ }; #endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 8913d4d1c..2d07bc49c 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -41,7 +41,6 @@ SRCS = navigator_main.cpp \ navigator_params.c \ mission.cpp \ rtl.cpp \ - rtl_params.c \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h new file mode 100644 index 000000000..1838fe32b --- /dev/null +++ b/src/modules/navigator/navigator.h @@ -0,0 +1,205 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator.h + * Helper class to access missions + * @author Julian Oes + * @author Anton Babushkin + */ + +#ifndef NAVIGATOR_H +#define NAVIGATOR_H + +#include + +#include +#include +#include +#include +#include + +#include "mission.h" +#include "rtl.h" +#include "geofence.h" + +class Navigator +{ +public: + /** + * Constructor + */ + Navigator(); + + /** + * Destructor, also kills the navigators task. + */ + ~Navigator(); + + /** + * Start the navigator task. + * + * @return OK on success. + */ + int start(); + + /** + * Display the navigator status. + */ + void status(); + + /** + * Add point to geofence + */ + void add_fence_point(int argc, char *argv[]); + + /** + * Load fence from file + */ + void load_fence_from_file(const char *filename); + + struct vehicle_status_s* get_vstatus() { return &_vstatus; } + struct vehicle_global_position_s* get_global_position() { return &_global_pos; } + struct home_position_s* get_home_position() { return &_home_pos; } + Geofence& get_geofence() { return _geofence; } + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _navigator_task; /**< task handle for sensor task */ + + int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ + + int _global_pos_sub; /**< global position subscription */ + int _home_pos_sub; /**< home position subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _offboard_mission_sub; /**< notification of offboard mission updates */ + int _onboard_mission_sub; /**< notification of onboard mission updates */ + int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _control_mode_sub; /**< vehicle control mode subscription */ + + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ + + vehicle_status_s _vstatus; /**< vehicle status */ + vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + vehicle_global_position_s _global_pos; /**< global vehicle position */ + home_position_s _home_pos; /**< home position for RTL */ + mission_item_s _mission_item; /**< current mission item */ + navigation_capabilities_s _nav_caps; /**< navigation capabilities */ + position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + + bool _mission_item_valid; /**< flags if the current mission item is valid */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + Geofence _geofence; /**< class that handles the geofence */ + bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ + + bool _fence_valid; /**< flag if fence is valid */ + bool _inside_fence; /**< vehicle is inside fence */ + + Mission _mission; /**< class that handles the missions */ + //Loiter _loiter; /**< class that handles loiter */ + RTL _rtl; /**< class that handles RTL */ + + bool _waypoint_position_reached; /**< flags if the position of the mission item has been reached */ + bool _waypoint_yaw_reached; /**< flags if the yaw position of the mission item has been reached */ + uint64_t _time_first_inside_orbit; /**< the time when we were first in the acceptance radius of the mission item */ + + bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ + + struct { + float acceptance_radius; + float loiter_radius; + int onboard_mission_enabled; + float takeoff_alt; + } _parameters; /**< local copies of parameters */ + + struct { + param_t acceptance_radius; + param_t loiter_radius; + param_t onboard_mission_enabled; + param_t takeoff_alt; + } _parameter_handles; /**< handles for parameters */ + + /** + * Update our local parameter cache. + */ + void parameters_update(); + + /** + * Retrieve global position + */ + void global_position_update(); + + /** + * Retrieve home position + */ + void home_position_update(); + + /** + * Retreive navigation capabilities + */ + void navigation_capabilities_update(); + + /** + * Retrieve vehicle status + */ + void vehicle_status_update(); + + /** + * Retrieve vehicle control mode + */ + void vehicle_control_mode_update(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main task. + */ + void task_main(); + + /** + * Translate mission item to a position setpoint. + */ + void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); + + /** + * Publish a new position setpoint triplet for position controllers + */ + void publish_position_setpoint_triplet(); +}; +#endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f30cd9a94..b991ffc8c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -62,11 +62,8 @@ #include #include -#include #include -#include #include -#include #include #include #include @@ -74,24 +71,13 @@ #include #include -#include #include #include #include #include #include -#include "mission.h" -#include "rtl.h" -#include "mission_feasibility_checker.h" -#include "geofence.h" - - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; +#include "navigator.h" /** * navigator app start / stop handling function @@ -100,174 +86,10 @@ static const int ERROR = -1; */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator -{ -public: - /** - * Constructor - */ - Navigator(); - - /** - * Destructor, also kills the navigators task. - */ - ~Navigator(); - - /** - * Start the navigator task. - * - * @return OK on success. - */ - int start(); - - /** - * Display the navigator status. - */ - void status(); - - /** - * Add point to geofence - */ - void add_fence_point(int argc, char *argv[]); - - /** - * Load fence from file - */ - void load_fence_from_file(const char *filename); - -private: - - bool _task_should_exit; /**< if true, sensor task should exit */ - int _navigator_task; /**< task handle for sensor task */ - - int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ - - int _global_pos_sub; /**< global position subscription */ - int _home_pos_sub; /**< home position subscription */ - int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _offboard_mission_sub; /**< notification of offboard mission updates */ - int _onboard_mission_sub; /**< notification of onboard mission updates */ - int _capabilities_sub; /**< notification of vehicle capabilities updates */ - int _control_mode_sub; /**< vehicle control mode subscription */ - - orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ - - vehicle_status_s _vstatus; /**< vehicle status */ - vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - vehicle_global_position_s _global_pos; /**< global vehicle position */ - home_position_s _home_pos; /**< home position for RTL */ - mission_item_s _mission_item; /**< current mission item */ - navigation_capabilities_s _nav_caps; /**< navigation capabilities */ - position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ - - bool _mission_item_valid; /**< flags if the current mission item is valid */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - Geofence _geofence; /**< class that handles the geofence */ - bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ - - bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ - - - Mission _mission; /**< class that handles the missions */ - - RTL _rtl; /**< class that handles RTL */ - - bool _waypoint_position_reached; /**< flags if the position of the mission item has been reached */ - bool _waypoint_yaw_reached; /**< flags if the yaw position of the mission item has been reached */ - uint64_t _time_first_inside_orbit; /**< the time when we were first in the acceptance radius of the mission item */ - - MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ - - bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ - - struct { - float acceptance_radius; - float loiter_radius; - int onboard_mission_enabled; - float takeoff_alt; - } _parameters; /**< local copies of parameters */ - - struct { - param_t acceptance_radius; - param_t loiter_radius; - param_t onboard_mission_enabled; - param_t takeoff_alt; - } _parameter_handles; /**< handles for parameters */ - - /** - * Update our local parameter cache. - */ - void parameters_update(); - - /** - * Retrieve global position - */ - void global_position_update(); - - /** - * Retrieve home position - */ - void home_position_update(); - - /** - * Retreive navigation capabilities - */ - void navigation_capabilities_update(); - - /** - * Retrieve offboard mission. - */ - void offboard_mission_update(); - - /** - * Retrieve onboard mission. - */ - void onboard_mission_update(); - - /** - * Retrieve vehicle status - */ - void vehicle_status_update(); - - /** - * Retrieve vehicle control mode - */ - void vehicle_control_mode_update(); - - /** - * Shim for calling task_main from task_create. - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * Main task. - */ - void task_main(); - - /** - * Translate mission item to a position setpoint. - */ - void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); - - /** - * Publish a new position setpoint triplet for position controllers - */ - void publish_position_setpoint_triplet(); -}; namespace navigator { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - Navigator *g_navigator; } @@ -299,8 +121,9 @@ Navigator::Navigator() : _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), - _mission({}), - _rtl({}), + _mission(this), + //_loiter(&_global_pos, &_home_pos, &_vstatus), + _rtl(this), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), @@ -351,7 +174,7 @@ Navigator::parameters_update() param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - _mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); + //_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); _geofence.updateParams(); } @@ -366,8 +189,6 @@ void Navigator::home_position_update() { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); - - _rtl.set_home_position(&_home_pos); } void @@ -376,54 +197,6 @@ Navigator::navigation_capabilities_update() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } - -void -Navigator::offboard_mission_update() -{ - struct mission_s offboard_mission; - - if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) { - - /* Check mission feasibility, for now do not handle the return value, - * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ - dm_item_t dm_current; - - if (offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - missionFeasiblityChecker.checkMissionFeasible(_vstatus.is_rotary_wing, dm_current, (size_t)offboard_mission.count, _geofence); - - _mission.set_offboard_dataman_id(offboard_mission.dataman_id); - - _mission.set_offboard_mission_count(offboard_mission.count); - _mission.command_current_offboard_mission_index(offboard_mission.current_index); - - } else { - _mission.set_offboard_mission_count(0); - _mission.command_current_offboard_mission_index(0); - } -} - -void -Navigator::onboard_mission_update() -{ - struct mission_s onboard_mission; - - if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { - - _mission.set_onboard_mission_count(onboard_mission.count); - _mission.command_current_onboard_mission_index(onboard_mission.current_index); - - } else { - _mission.set_onboard_mission_count(0); - _mission.command_current_onboard_mission_index(0); - } -} - void Navigator::vehicle_status_update() { @@ -459,8 +232,7 @@ Navigator::task_main() /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file - * else clear geofence data in datamanager - */ + * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { @@ -474,9 +246,7 @@ Navigator::task_main() warnx("Could not clear geofence"); } - /* - * do subscriptions - */ + /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); @@ -493,8 +263,6 @@ Navigator::task_main() global_position_update(); home_position_update(); navigation_capabilities_update(); - offboard_mission_update(); - onboard_mission_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -503,7 +271,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[8]; + struct pollfd fds[6]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -514,14 +282,10 @@ Navigator::task_main() fds[2].events = POLLIN; fds[3].fd = _capabilities_sub; fds[3].events = POLLIN; - fds[4].fd = _offboard_mission_sub; + fds[4].fd = _vstatus_sub; fds[4].events = POLLIN; - fds[5].fd = _onboard_mission_sub; + fds[5].fd = _control_mode_sub; fds[5].events = POLLIN; - fds[6].fd = _vstatus_sub; - fds[6].events = POLLIN; - fds[7].fd = _control_mode_sub; - fds[7].events = POLLIN; while (!_task_should_exit) { @@ -547,12 +311,12 @@ Navigator::task_main() } /* vehicle control mode updated */ - if (fds[7].revents & POLLIN) { + if (fds[5].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ - if (fds[6].revents & POLLIN) { + if (fds[4].revents & POLLIN) { vehicle_status_update(); } @@ -567,16 +331,6 @@ Navigator::task_main() navigation_capabilities_update(); } - /* offboard mission updated */ - if (fds[4].revents & POLLIN) { - offboard_mission_update(); - } - - /* onboard mission updated */ - if (fds[5].revents & POLLIN) { - onboard_mission_update(); - } - /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); @@ -600,6 +354,33 @@ Navigator::task_main() } } + /* Do stuff according to navigation state set by commander */ + switch (_vstatus.set_nav_state) { + case NAVIGATION_STATE_MANUAL: + case NAVIGATION_STATE_ACRO: + case NAVIGATION_STATE_ALTCTL: + case NAVIGATION_STATE_POSCTL: + break; + case NAVIGATION_STATE_AUTO_MISSION: + _update_triplet = _mission.update(&_pos_sp_triplet); + _rtl.reset(); + break; + case NAVIGATION_STATE_AUTO_LOITER: + //_loiter.update(); + break; + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTL_RC: + case NAVIGATION_STATE_AUTO_RTL_DL: + _mission.reset(); + _update_triplet = _rtl.update(&_pos_sp_triplet); + break; + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: + default: + break; + + } + if (_update_triplet ) { publish_position_setpoint_triplet(); _update_triplet = false; @@ -880,47 +661,6 @@ Navigator::start_land() return true; } #endif -void -Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) -{ - sp->valid = true; - - if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - - if (_pos_sp_triplet.previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon); - - } else { - /* else use current position */ - sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon); - } - - } else { - sp->lat = item->lat; - sp->lon = item->lon; - sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; - sp->yaw = item->yaw; - sp->loiter_radius = item->loiter_radius; - sp->loiter_direction = item->loiter_direction; - sp->pitch_min = item->pitch_min; - } - - if (item->nav_cmd == NAV_CMD_TAKEOFF) { - sp->type = SETPOINT_TYPE_TAKEOFF; - - } else if (item->nav_cmd == NAV_CMD_LAND) { - sp->type = SETPOINT_TYPE_LAND; - - } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - sp->type = SETPOINT_TYPE_LOITER; - - } else { - sp->type = SETPOINT_TYPE_NORMAL; - } -} #if 0 bool Navigator::check_mission_item_reached() diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b8ea06275..9d7886aa6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -51,26 +51,41 @@ #include "rtl.h" - -RTL::RTL() : - SuperBlock(NULL, "RTL"), +RTL::RTL(Navigator *navigator) : + Mission(navigator), _mavlink_fd(-1), _rtl_state(RTL_STATE_NONE), _home_position({}), + _loiter_radius(50), + _acceptance_radius(50), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY"), - _loiter_radius(50), - _acceptance_radius(50) + _param_land_delay(this, "LAND_DELAY") { /* load initial params */ updateParams(); + /* initial reset */ + reset(); } RTL::~RTL() { } +bool +RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + bool updated = false; + + return updated; +} + +void +RTL::reset() +{ + +} + void RTL::set_home_position(const home_position_s *new_home_position) { @@ -206,7 +221,7 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss } default: - return false; + return false; } return true; @@ -226,7 +241,7 @@ RTL::move_to_next() case RTL_STATE_CLIMB: _rtl_state = RTL_STATE_RETURN; break; - + case RTL_STATE_RETURN: _rtl_state = RTL_STATE_DESCEND; break; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index c761837fc..8d1bfec59 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/*************************************************************************** * * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. * @@ -43,23 +43,33 @@ #include #include +#include #include #include #include -class RTL : public control::SuperBlock +#include "mission.h" + +class Navigator; + +class RTL : public Mission { public: /** * Constructor */ - RTL(); + RTL(Navigator *navigator); /** * Destructor */ - ~RTL(); + virtual ~RTL(); + + virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void reset(); + +private: void set_home_position(const home_position_s *home_position); bool get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item); @@ -67,7 +77,6 @@ public: void move_to_next(); -private: int _mavlink_fd; enum RTLState { @@ -77,7 +86,7 @@ private: RTL_STATE_DESCEND, RTL_STATE_LAND, RTL_STATE_FINISHED, - } _rtl_state; + } _rtl_state; home_position_s _home_position; float _loiter_radius; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 66a9e31c6..2b6caf159 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1340,7 +1340,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; + log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */ log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); log_msg.body.log_GPSP.alt = buf.triplet.current.alt; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 4532b329d..d25db3a77 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,9 @@ /** * @file mission.h * Definition of a mission consisting of mission items. + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier */ #ifndef TOPIC_MISSION_H_ @@ -92,9 +92,9 @@ struct mission_item_s { float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ bool autocontinue; /**< true if next waypoint should follow after this one */ enum ORIGIN origin; /**< where the waypoint has been generated */ - int do_jump_mission_index; /**< the mission index that we want to jump to */ - int do_jump_repeat_count; /**< how many times the jump should be repeated */ - int do_jump_current_count; /**< how many times the jump has already been repeated */ + int do_jump_mission_index; /**< index where the do jump will go to */ + unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */ + unsigned do_jump_current_count; /**< count how many times the jump has been done */ }; struct mission_s diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index c2741a05b..ce42035ba 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,10 @@ /** * @file mission_item_triplet.h * Definition of the global WGS84 position setpoint uORB topic. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier */ #ifndef TOPIC_MISSION_ITEM_TRIPLET_H_ @@ -53,11 +54,12 @@ enum SETPOINT_TYPE { - SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */ - SETPOINT_TYPE_LOITER, /**< loiter setpoint */ - SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ - SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, vehicle must descend until landing */ - SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ + SETPOINT_TYPE_POSITION = 0, /**< position setpoint */ + SETPOINT_TYPE_VELOCITY, /**< velocity setpoint */ + SETPOINT_TYPE_LOITER, /**< loiter setpoint */ + SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ + SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ + SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ }; struct position_setpoint_s @@ -73,16 +75,6 @@ struct position_setpoint_s float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ }; -typedef enum { - NAV_STATE_MANUAL = 0, - NAV_STATE_POSCTL, - NAV_STATE_AUTO, - NAV_STATE_RC_LOSS, - NAV_STATE_DL_LOSS, - NAV_STATE_TERMINATION, - MAX_NAV_STATE -} nav_state_t; - /** * Global position setpoint triplet in WGS84 coordinates. * @@ -93,8 +85,6 @@ struct position_setpoint_triplet_s struct position_setpoint_s previous; struct position_setpoint_s current; struct position_setpoint_s next; - - nav_state_t nav_state; /**< navigation state */ }; /** -- cgit v1.2.3 From 94bed70e32a7f4815606a22693fae64060a053ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Jun 2014 16:37:58 +0200 Subject: Reworked the estimator initialization and recovery logic. Should be more resilient to mishaps now --- .../ekf_att_pos_estimator_main.cpp | 524 +++++++++++---------- src/modules/ekf_att_pos_estimator/estimator.cpp | 16 +- src/modules/ekf_att_pos_estimator/estimator.h | 1 - 3 files changed, 281 insertions(+), 260 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 93ed18b8d..0a5b26f4a 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -577,6 +577,11 @@ FixedwingEstimator::task_main() bool newAdsData = false; bool newDataMag = false; + float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m) + _gps.vel_n_m_s = 0.0f; + _gps.vel_e_m_s = 0.0f; + _gps.vel_d_m_s = 0.0f; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -926,8 +931,15 @@ FixedwingEstimator::task_main() newDataMag = false; } + /* + * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY + */ + if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) { + continue; + } - /** + + /* * CHECK IF THE INPUT DATA IS SANE */ int check = _ekf->CheckAndBound(); @@ -959,6 +971,13 @@ FixedwingEstimator::task_main() mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); break; } + case 4: + { + const char* str = "excessive gyro offsets"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } default: { @@ -974,7 +993,7 @@ FixedwingEstimator::task_main() } // If non-zero, we got a filter reset - if (check) { + if (check > 0 && check != 3) { struct ekf_status_report ekf_report; @@ -1013,10 +1032,12 @@ FixedwingEstimator::task_main() _baro_init = false; _gps_initialized = false; + _initialized = false; last_sensor_timestamp = hrt_absolute_time(); last_run = last_sensor_timestamp; _ekf->ZeroVariables(); + _ekf->statesInitialised = false; _ekf->dtIMU = 0.01f; // Let the system re-initialize itself @@ -1027,23 +1048,26 @@ FixedwingEstimator::task_main() /** * PART TWO: EXECUTE THE FILTER + * + * We run the filter only once all data has been fetched **/ - if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { float initVelNED[3]; + /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { - initVelNED[0] = _gps.vel_n_m_s; - initVelNED[1] = _gps.vel_e_m_s; - initVelNED[2] = _gps.vel_d_m_s; - // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; double lon = _gps.lon / 1.0e7; float gps_alt = _gps.alt / 1e3f; + initVelNED[0] = _gps.vel_n_m_s; + initVelNED[1] = _gps.vel_e_m_s; + initVelNED[2] = _gps.vel_d_m_s; + // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_gps_offset = _baro_ref - _baro.altitude; @@ -1070,10 +1094,13 @@ FixedwingEstimator::task_main() map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + + #if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); + #endif _gps_initialized = true; @@ -1082,282 +1109,268 @@ FixedwingEstimator::task_main() initVelNED[0] = 0.0f; initVelNED[1] = 0.0f; initVelNED[2] = 0.0f; - _ekf->posNED[0] = 0.0f; - _ekf->posNED[1] = 0.0f; - _ekf->posNED[2] = 0.0f; - - _ekf->posNE[0] = _ekf->posNED[0]; - _ekf->posNE[1] = _ekf->posNED[1]; + _ekf->posNE[0] = posNED[0]; + _ekf->posNE[1] = posNED[1]; _local_pos.ref_alt = _baro_ref; _baro_gps_offset = 0.0f; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - } - } - - // If valid IMU data and states initialised, predict states and covariances - if (_ekf->statesInitialised) { - // Run the strapdown INS equations every IMU update - _ekf->UpdateStrapdownEquationsNED(); -#if 0 - // debug code - could be tunred into a filter mnitoring/watchdog function - float tempQuat[4]; + } else if (_ekf->statesInitialised) { - for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; + // We're apparently initialized in this case now - quat2eul(eulerEst, tempQuat); - for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; + // Run the strapdown INS equations every IMU update + _ekf->UpdateStrapdownEquationsNED(); + #if 0 + // debug code - could be tunred into a filter mnitoring/watchdog function + float tempQuat[4]; - if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; + for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; - if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; + quat2eul(eulerEst, tempQuat); -#endif - // store the predicted states for subsequent use by measurement fusion - _ekf->StoreStates(IMUmsec); - // Check if on ground - status is used by covariance prediction - _ekf->OnGroundCheck(); - // sum delta angles and time used by covariance prediction - _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; - _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; - dt += _ekf->dtIMU; - - // perform a covariance prediction if the total delta angle has exceeded the limit - // or the time limit will be exceeded at the next IMU update - if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { - _ekf->CovariancePrediction(dt); - _ekf->summedDelAng.zero(); - _ekf->summedDelVel.zero(); - dt = 0.0f; - } + for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; - _initialized = true; - } + if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; - // Fuse GPS Measurements - if (newDataGps && _gps_initialized) { - // Convert GPS measurements to Pos NE, hgt and Vel NED - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - - _ekf->posNE[0] = _ekf->posNED[0]; - _ekf->posNE[1] = _ekf->posNED[1]; - // set fusion flags - _ekf->fuseVelData = true; - _ekf->fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else if (_ekf->statesInitialised) { - // Convert GPS measurements to Pos NE, hgt and Vel NED - _ekf->velNED[0] = 0.0f; - _ekf->velNED[1] = 0.0f; - _ekf->velNED[2] = 0.0f; - _ekf->posNED[0] = 0.0f; - _ekf->posNED[1] = 0.0f; - _ekf->posNED[2] = 0.0f; - - _ekf->posNE[0] = _ekf->posNED[0]; - _ekf->posNE[1] = _ekf->posNED[1]; - // set fusion flags - _ekf->fuseVelData = true; - _ekf->fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else { - _ekf->fuseVelData = false; - _ekf->fusePosData = false; - } - - if (newHgtData && _ekf->statesInitialised) { - // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); - _ekf->fuseHgtData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else { - _ekf->fuseHgtData = false; - } - - // Fuse Magnetometer Measurements - if (newDataMag && _ekf->statesInitialised) { - _ekf->fuseMagData = true; - _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data - - } else { - _ekf->fuseMagData = false; - } + if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; - if (_ekf->statesInitialised) _ekf->FuseMagnetometer(); - - // Fuse Airspeed Measurements - if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { - _ekf->fuseVtasData = true; - _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - _ekf->FuseAirspeed(); - - } else { - _ekf->fuseVtasData = false; - } - - // Publish results - if (_initialized && (check == OK)) { - - - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) - - math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); - math::Matrix<3, 3> R = q.to_dcm(); - math::Vector<3> euler = R.to_euler(); - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = R(i, j); - - _att.timestamp = last_sensor_timestamp; - _att.q[0] = _ekf->states[0]; - _att.q[1] = _ekf->states[1]; - _att.q[2] = _ekf->states[2]; - _att.q[3] = _ekf->states[3]; - _att.q_valid = true; - _att.R_valid = true; - - _att.timestamp = last_sensor_timestamp; - _att.roll = euler(0); - _att.pitch = euler(1); - _att.yaw = euler(2); - - _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; - // gyro offsets - _att.rate_offsets[0] = _ekf->states[10]; - _att.rate_offsets[1] = _ekf->states[11]; - _att.rate_offsets[2] = _ekf->states[12]; - - /* lazily publish the attitude only once available */ - if (_att_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - - } else { - /* advertise and publish */ - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); - } - } - - if (_gps_initialized) { - _local_pos.timestamp = last_sensor_timestamp; - _local_pos.x = _ekf->states[7]; - _local_pos.y = _ekf->states[8]; - // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_gps_offset; + #endif + // store the predicted states for subsequent use by measurement fusion + _ekf->StoreStates(IMUmsec); + // Check if on ground - status is used by covariance prediction + _ekf->OnGroundCheck(); + // sum delta angles and time used by covariance prediction + _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; + _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; + dt += _ekf->dtIMU; + + // perform a covariance prediction if the total delta angle has exceeded the limit + // or the time limit will be exceeded at the next IMU update + if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { + _ekf->CovariancePrediction(dt); + _ekf->summedDelAng.zero(); + _ekf->summedDelVel.zero(); + dt = 0.0f; + } - _local_pos.vx = _ekf->states[4]; - _local_pos.vy = _ekf->states[5]; - _local_pos.vz = _ekf->states[6]; + _initialized = true; + + // Fuse GPS Measurements + if (newDataGps && _gps_initialized) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; + _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); + + _ekf->posNE[0] = posNED[0]; + _ekf->posNE[1] = posNED[1]; + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); + + } else if (_ekf->statesInitialised) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + _ekf->velNED[0] = 0.0f; + _ekf->velNED[1] = 0.0f; + _ekf->velNED[2] = 0.0f; + + _ekf->posNE[0] = 0.0f; + _ekf->posNE[1] = 0.0f; + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); - _local_pos.xy_valid = _gps_initialized; - _local_pos.z_valid = true; - _local_pos.v_xy_valid = _gps_initialized; - _local_pos.v_z_valid = true; - _local_pos.xy_global = true; + } else { + _ekf->fuseVelData = false; + _ekf->fusePosData = false; + } - _local_pos.z_global = false; - _local_pos.yaw = _att.yaw; + if (newHgtData && _ekf->statesInitialised) { + // Could use a blend of GPS and baro alt data if desired + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + _ekf->fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); - /* lazily publish the local position only once available */ - if (_local_pos_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); + } else { + _ekf->fuseHgtData = false; + } - } else { - /* advertise and publish */ - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); - } + // Fuse Magnetometer Measurements + if (newDataMag && _ekf->statesInitialised) { + _ekf->fuseMagData = true; + _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data - _global_pos.timestamp = _local_pos.timestamp; + } else { + _ekf->fuseMagData = false; + } - if (_local_pos.xy_global) { - double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); - _global_pos.lat = est_lat; - _global_pos.lon = est_lon; - _global_pos.time_gps_usec = _gps.time_gps_usec; - _global_pos.eph = _gps.eph_m; - _global_pos.epv = _gps.epv_m; - } + if (_ekf->statesInitialised) _ekf->FuseMagnetometer(); - if (_local_pos.v_xy_valid) { - _global_pos.vel_n = _local_pos.vx; - _global_pos.vel_e = _local_pos.vy; - } else { - _global_pos.vel_n = 0.0f; - _global_pos.vel_e = 0.0f; - } + // Fuse Airspeed Measurements + if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { + _ekf->fuseVtasData = true; + _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + _ekf->FuseAirspeed(); - /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z); + } else { + _ekf->fuseVtasData = false; + } - if (_local_pos.v_z_valid) { - _global_pos.vel_d = _local_pos.vz; - } - _global_pos.yaw = _local_pos.yaw; + // Output results + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); + math::Matrix<3, 3> R = q.to_dcm(); + math::Vector<3> euler = R.to_euler(); + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _att.R[i][j] = R(i, j); + + _att.timestamp = last_sensor_timestamp; + _att.q[0] = _ekf->states[0]; + _att.q[1] = _ekf->states[1]; + _att.q[2] = _ekf->states[2]; + _att.q[3] = _ekf->states[3]; + _att.q_valid = true; + _att.R_valid = true; + + _att.timestamp = last_sensor_timestamp; + _att.roll = euler(0); + _att.pitch = euler(1); + _att.yaw = euler(2); + + _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; + // gyro offsets + _att.rate_offsets[0] = _ekf->states[10]; + _att.rate_offsets[1] = _ekf->states[11]; + _att.rate_offsets[2] = _ekf->states[12]; + + /* lazily publish the attitude only once available */ + if (_att_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - _global_pos.eph = _gps.eph_m; - _global_pos.epv = _gps.epv_m; + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } - _global_pos.timestamp = _local_pos.timestamp; + if (_gps_initialized) { + _local_pos.timestamp = last_sensor_timestamp; + _local_pos.x = _ekf->states[7]; + _local_pos.y = _ekf->states[8]; + // XXX need to announce change of Z reference somehow elegantly + _local_pos.z = _ekf->states[9] - _baro_gps_offset; + + _local_pos.vx = _ekf->states[4]; + _local_pos.vy = _ekf->states[5]; + _local_pos.vz = _ekf->states[6]; + + _local_pos.xy_valid = _gps_initialized; + _local_pos.z_valid = true; + _local_pos.v_xy_valid = _gps_initialized; + _local_pos.v_z_valid = true; + _local_pos.xy_global = true; + + _local_pos.z_global = false; + _local_pos.yaw = _att.yaw; + + /* lazily publish the local position only once available */ + if (_local_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); + + } else { + /* advertise and publish */ + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); + } + + _global_pos.timestamp = _local_pos.timestamp; + + if (_local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + _global_pos.lat = est_lat; + _global_pos.lon = est_lon; + _global_pos.time_gps_usec = _gps.time_gps_usec; + _global_pos.eph = _gps.eph_m; + _global_pos.epv = _gps.epv_m; + } + + if (_local_pos.v_xy_valid) { + _global_pos.vel_n = _local_pos.vx; + _global_pos.vel_e = _local_pos.vy; + } else { + _global_pos.vel_n = 0.0f; + _global_pos.vel_e = 0.0f; + } + + /* local pos alt is negative, change sign and add alt offsets */ + _global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z); + + if (_local_pos.v_z_valid) { + _global_pos.vel_d = _local_pos.vz; + } + + + _global_pos.yaw = _local_pos.yaw; + + _global_pos.eph = _gps.eph_m; + _global_pos.epv = _gps.epv_m; + + _global_pos.timestamp = _local_pos.timestamp; + + /* lazily publish the global position only once available */ + if (_global_pos_pub > 0) { + /* publish the global position */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } + + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + _wind.covariance_north = 0.0f; // XXX get form filter + _wind.covariance_east = 0.0f; + + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } + + } - /* lazily publish the global position only once available */ - if (_global_pos_pub > 0) { - /* publish the global position */ - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + } - } else { - /* advertise and publish */ - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = 0.0f; // XXX get form filter - _wind.covariance_east = 0.0f; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - } } } @@ -1407,9 +1420,10 @@ FixedwingEstimator::print_status() // 4-6: Velocity - m/sec (North, East, Down) // 7-9: Position - m (North, East, Down) // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + // 13: Accelerometer offset + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 1320b4668..15dbd0597 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -145,7 +145,7 @@ AttPosEKF::AttPosEKF() * instead to allow clean in-air re-initialization. */ { - + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); ZeroVariables(); InitialiseParameters(); } @@ -2382,7 +2382,7 @@ int AttPosEKF::CheckAndBound() // Reset the filter if the IMU data is too old if (dtIMU > 0.3f) { - + FillErrorReport(&last_ekf_error); ResetVelocity(); ResetPosition(); ResetHeight(); @@ -2397,6 +2397,7 @@ int AttPosEKF::CheckAndBound() // Check if we switched between states if (currStaticMode != staticMode) { + FillErrorReport(&last_ekf_error); ResetVelocity(); ResetPosition(); ResetHeight(); @@ -2405,6 +2406,15 @@ int AttPosEKF::CheckAndBound() return 3; } + // Reset the filter if gyro offsets are excessive + if (fabs(states[10]) > 1.0f || fabsf(states[11]) > 1.0f || fabsf(states[12]) > 1.0f) { + + InitializeDynamic(velNED, magDeclination); + + // that's all we can do here, return + return 4; + } + return 0; } @@ -2531,8 +2541,6 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do // the baro offset must be this difference now baroHgtOffset = baroHgt - referenceHgt; - memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - InitializeDynamic(initvelNED, declination); } diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e821089f2..ec82896fb 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -200,7 +200,6 @@ public: float hgtMea; // measured height (m) float baroHgtOffset; ///< the baro (weather) offset from normalized altitude float rngMea; // Ground distance - float posNED[3]; // North, East Down position (m) float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output -- cgit v1.2.3 From ed65d1748e55e156dbf2067521685f804d5aefd6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Jun 2014 17:55:33 +0200 Subject: Fix initialization of position variable --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 15dbd0597..bfb2007af 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2537,6 +2537,8 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do // we are at reference altitude, so measurement must be zero hgtMea = 0.0f; + posNE[0] = 0.0f; + posNE[1] = 0.0f; // the baro offset must be this difference now baroHgtOffset = baroHgt - referenceHgt; -- cgit v1.2.3 From 82b7b80f475d12fc31eb63c0d69a7cf8f75ac534 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Jun 2014 21:20:44 +0200 Subject: navigator: bugfixing (WIP: mission topic not copying) --- src/modules/navigator/mission.cpp | 37 ++++++++++++++++++++++++-------- src/modules/navigator/mission.h | 2 ++ src/modules/navigator/navigator.h | 2 -- src/modules/navigator/navigator_main.cpp | 9 +++----- src/modules/navigator/rtl.cpp | 4 +--- 5 files changed, 34 insertions(+), 20 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index f3a86666f..7e02f8c15 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -50,6 +50,7 @@ #include #include +#include #include #include "navigator.h" @@ -61,8 +62,6 @@ Mission::Mission(Navigator *navigator) : _navigator(navigator), _first_run(true), _param_onboard_enabled(this, "ONBOARD_EN"), - _onboard_mission_sub(-1), - _offboard_mission_sub(-1), _onboard_mission({0}), _offboard_mission({0}), _mission_item({0}), @@ -74,6 +73,9 @@ Mission::Mission(Navigator *navigator) : updateParams(); /* set initial mission items */ reset(); + + _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); } Mission::~Mission() @@ -89,9 +91,14 @@ Mission::reset() bool Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { + /* check if anything has changed */ + bool onboard_updated = false; //is_onboard_mission_updated(); + bool offboard_updated = is_offboard_mission_updated(); + bool updated = false; - /* check if anything has changed, and reset mission items if needed */ - if (is_onboard_mission_updated() || is_offboard_mission_updated() || _first_run) { + + /* reset mission items if needed */ + if (onboard_updated || offboard_updated) { set_mission_items(pos_sp_triplet); updated = true; _first_run = false; @@ -229,7 +236,7 @@ Mission::is_onboard_mission_updated() bool updated; orb_check(_onboard_mission_sub, &updated); - if (!updated) { + if (!updated && !_first_run) { return false; } @@ -244,12 +251,17 @@ bool Mission::is_offboard_mission_updated() { bool updated; + warnx("sub: %d", _offboard_mission_sub); orb_check(_offboard_mission_sub, &updated); - if (!updated) { + if (!updated && !_first_run) { + warnx("not updated"); return false; } - - if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) { + struct mission_s offboard_mission; + int ret = orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission); + warnx("ret: %d", ret); + if (ret == OK) { + warnx("copy new offboard mission"); /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ @@ -263,8 +275,10 @@ Mission::is_offboard_mission_updated() missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t)_offboard_mission.count, - _navigator->get_geofence()); + _navigator->get_geofence(), + _navigator->get_home_position()->alt); } else { + warnx("no success with orb_copy"); _offboard_mission.count = 0; _offboard_mission.current_index = 0; } @@ -341,7 +355,9 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current bool Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { + warnx("try offboard mission: %d, %d", _offboard_mission.current_index, _offboard_mission.count ); if (_offboard_mission.current_index < (int)_offboard_mission.count) { + warnx("theoretically possible"); dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -356,8 +372,11 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren report_current_offboard_mission_item(); return true; + } else { + warnx("read fail"); } } + warnx("failed with offboard mission"); return false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index e86dd25bb..65a0991b5 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -47,9 +47,11 @@ #include +#include #include #include #include +#include #include #include "mission_feasibility_checker.h" diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 1838fe32b..927e77391 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -103,8 +103,6 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ - int _offboard_mission_sub; /**< notification of offboard mission updates */ - int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f91032196..9dd253127 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -103,8 +103,6 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _params_sub(-1), - _offboard_mission_sub(-1), - _onboard_mission_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), _pos_sp_triplet_pub(-1), @@ -248,8 +246,6 @@ Navigator::task_main() /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); - _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -360,6 +356,8 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: + _mission.reset(); + _rtl.reset(); break; case NAVIGATION_STATE_AUTO_MISSION: _update_triplet = _mission.update(&_pos_sp_triplet); @@ -660,8 +658,6 @@ Navigator::start_land() _update_triplet = true; return true; } -#endif -#if 0 bool Navigator::check_mission_item_reached() { @@ -766,6 +762,7 @@ Navigator::reset_reached() _waypoint_yaw_reached = false; } +#endif void Navigator::publish_position_setpoint_triplet() { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 9d7886aa6..dc87b0521 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -57,9 +57,7 @@ RTL::RTL(Navigator *navigator) : _rtl_state(RTL_STATE_NONE), _home_position({}), _loiter_radius(50), - _acceptance_radius(50), - _param_return_alt(this, "RETURN_ALT"), - _param_descend_alt(this, "DESCEND_ALT"), + _acceptance_radius(50) _param_land_delay(this, "LAND_DELAY") { /* load initial params */ -- cgit v1.2.3 From 5ed3652c2fdc20ba70df1cde6c28d58fa6be0e04 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Jun 2014 21:29:26 +0200 Subject: navigator: compile fix --- src/modules/navigator/rtl.cpp | 4 +++- src/modules/navigator/rtl.h | 1 - 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index dc87b0521..9d7886aa6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -57,7 +57,9 @@ RTL::RTL(Navigator *navigator) : _rtl_state(RTL_STATE_NONE), _home_position({}), _loiter_radius(50), - _acceptance_radius(50) + _acceptance_radius(50), + _param_return_alt(this, "RETURN_ALT"), + _param_descend_alt(this, "DESCEND_ALT"), _param_land_delay(this, "LAND_DELAY") { /* load initial params */ diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 8d1bfec59..60336d5d0 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -92,7 +92,6 @@ private: float _loiter_radius; float _acceptance_radius; - control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; -- cgit v1.2.3 From 93295861c532595e080ae61c32d5e2cb625b4eac Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Jun 2014 01:07:50 +0200 Subject: navigator: missions work again, loiter when finished or no mission available or sd card removed works as well --- src/modules/navigator/mission.cpp | 133 +++++++++++++++++++++++-------- src/modules/navigator/mission.h | 16 +++- src/modules/navigator/mission_params.c | 68 ++++++++++++++++ src/modules/navigator/module.mk | 1 + src/modules/navigator/navigator.h | 7 ++ src/modules/navigator/navigator_main.cpp | 4 + 6 files changed, 191 insertions(+), 38 deletions(-) create mode 100644 src/modules/navigator/mission_params.c diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7e02f8c15..38badcff9 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -62,20 +62,22 @@ Mission::Mission(Navigator *navigator) : _navigator(navigator), _first_run(true), _param_onboard_enabled(this, "ONBOARD_EN"), + _param_loiter_radius(this, "LOITER_RAD"), _onboard_mission({0}), _offboard_mission({0}), + _current_onboard_mission_index(-1), + _current_offboard_mission_index(-1), _mission_item({0}), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE) + _mission_type(MISSION_TYPE_NONE), + _loiter_set(false) { /* load initial params */ updateParams(); /* set initial mission items */ reset(); - _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); - _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); } Mission::~Mission() @@ -86,13 +88,15 @@ void Mission::reset() { _first_run = true; + _loiter_set = false; } bool Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { + /* check if anything has changed */ - bool onboard_updated = false; //is_onboard_mission_updated(); + bool onboard_updated = is_onboard_mission_updated(); bool offboard_updated = is_offboard_mission_updated(); bool updated = false; @@ -104,11 +108,20 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) _first_run = false; } + /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { advance_mission(); set_mission_items(pos_sp_triplet); updated = true; } + + /* maybe we couldn't actually set a mission, therefore lets set a loiter setpoint */ + if (_mission_type == MISSION_TYPE_NONE && !_loiter_set) { + bool use_current_pos_sp = (pos_sp_triplet->current.valid && _waypoint_position_reached); + set_loiter_item(use_current_pos_sp, pos_sp_triplet); + updated = true; + _loiter_set = true; + } return updated; } @@ -195,17 +208,17 @@ Mission::is_mission_item_reached() } void -Mission::reset_mission_item_reached() { +Mission::reset_mission_item_reached() +{ _waypoint_position_reached = false; _waypoint_yaw_reached = false; _time_first_inside_orbit = 0; } void -Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) +Mission::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) { sp->valid = true; - sp->lat = item->lat; sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; @@ -230,19 +243,56 @@ Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_ } } +void +Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet) +{ + if (reuse_current_pos_sp && pos_sp_triplet->current.valid) { + /* nothing to be done, just use the current item */ + } else { + pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; + pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */ + } + pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; + pos_sp_triplet->current.loiter_radius = _param_loiter_radius.get(); + pos_sp_triplet->current.loiter_direction = 1; + + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; +} + + bool Mission::is_onboard_mission_updated() { bool updated; - orb_check(_onboard_mission_sub, &updated); + orb_check(_navigator->get_onboard_mission_sub(), &updated); if (!updated && !_first_run) { return false; } - if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) { + if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { + /* accept the current index set by the onboard mission if it is within bounds */ + if (_onboard_mission.current_index >=0 + && _onboard_mission.current_index < (int)_onboard_mission.count) { + _current_onboard_mission_index = _onboard_mission.current_index; + } else { + /* if less WPs available, reset to first WP */ + if (_current_onboard_mission_index >= (int)_onboard_mission.count) { + _current_onboard_mission_index = 0; + /* if not initialized, set it to 0 */ + } else if (_current_onboard_mission_index < 0) { + _current_onboard_mission_index = 0; + } + /* otherwise, just leave it */ + } + } else { _onboard_mission.count = 0; _onboard_mission.current_index = 0; + _current_onboard_mission_index = 0; } return true; } @@ -251,17 +301,27 @@ bool Mission::is_offboard_mission_updated() { bool updated; - warnx("sub: %d", _offboard_mission_sub); - orb_check(_offboard_mission_sub, &updated); + orb_check(_navigator->get_offboard_mission_sub(), &updated); + if (!updated && !_first_run) { - warnx("not updated"); return false; } - struct mission_s offboard_mission; - int ret = orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission); - warnx("ret: %d", ret); - if (ret == OK) { - warnx("copy new offboard mission"); + if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { + + /* determine current index */ + if (_offboard_mission.current_index >= 0 + && _offboard_mission.current_index < (int)_offboard_mission.count) { + _current_offboard_mission_index = _offboard_mission.current_index; + } else { + /* if less WPs available, reset to first WP */ + if (_current_offboard_mission_index >= (int)_offboard_mission.count) { + _current_offboard_mission_index = 0; + /* if not initialized, set it to 0 */ + } else if (_current_offboard_mission_index < 0) { + _current_offboard_mission_index = 0; + } + /* otherwise, just leave it */ + } /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ @@ -278,9 +338,9 @@ Mission::is_offboard_mission_updated() _navigator->get_geofence(), _navigator->get_home_position()->alt); } else { - warnx("no success with orb_copy"); _offboard_mission.count = 0; _offboard_mission.current_index = 0; + _current_offboard_mission_index = 0; } return true; } @@ -291,11 +351,11 @@ Mission::advance_mission() { switch (_mission_type) { case MISSION_TYPE_ONBOARD: - _onboard_mission.current_index++; + _current_onboard_mission_index++; break; case MISSION_TYPE_OFFBOARD: - _offboard_mission.current_index++; + _current_offboard_mission_index++; break; case MISSION_TYPE_NONE: @@ -307,17 +367,17 @@ Mission::advance_mission() void Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) { - warnx("set mission items"); - set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { /* try setting onboard mission item */ _mission_type = MISSION_TYPE_ONBOARD; + _loiter_set = false; } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { /* try setting offboard mission item */ _mission_type = MISSION_TYPE_OFFBOARD; + _loiter_set = false; } else { _mission_type = MISSION_TYPE_NONE; } @@ -337,15 +397,18 @@ bool Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { if (_param_onboard_enabled.get() > 0 - && _onboard_mission.current_index < (int)_onboard_mission.count) { + && _current_onboard_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index, + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, &new_mission_item)) { /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&_mission_item, current_pos_sp); + mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); current_pos_sp->valid = true; + reset_mission_item_reached(); + /* TODO: report this somehow */ + memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); return true; } } @@ -355,9 +418,7 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current bool Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { - warnx("try offboard mission: %d, %d", _offboard_mission.current_index, _offboard_mission.count ); - if (_offboard_mission.current_index < (int)_offboard_mission.count) { - warnx("theoretically possible"); + if (_current_offboard_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -365,18 +426,20 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } struct mission_item_s new_mission_item; - if (read_mission_item(dm_current, true, &_offboard_mission.current_index, &new_mission_item)) { + if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) { /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&_mission_item, current_pos_sp); + mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); current_pos_sp->valid = true; + reset_mission_item_reached(); + report_current_offboard_mission_item(); + memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); return true; } else { - warnx("read fail"); + warnx("ERROR: WP read fail"); } } - warnx("failed with offboard mission"); return false; } @@ -443,6 +506,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + /* TODO: do this check more gracefully since it is not a serious error */ if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { return false; } @@ -458,6 +522,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* not supposed to happen unless the datamanager can't access the dataman */ return false; } + /* TODO: report about DO JUMP count */ } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ @@ -479,7 +544,7 @@ Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _offboard_mission.current_index; + _mission_result.mission_index_reached = _current_offboard_mission_index; } publish_mission_result(); } @@ -487,7 +552,7 @@ Mission::report_mission_item_reached() void Mission::report_current_offboard_mission_item() { - _mission_result.index_current_mission = _offboard_mission.current_index; + _mission_result.index_current_mission = _current_offboard_mission_index; publish_mission_result(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 65a0991b5..f75c8a882 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -58,7 +58,7 @@ class Navigator; -class Mission : public control::SuperBlock +class __EXPORT Mission : public control::SuperBlock { public: /** @@ -97,6 +97,11 @@ protected: */ void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp); + /** + * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position + */ + void set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet); + class Navigator *_navigator; bool _waypoint_position_reached; @@ -179,13 +184,14 @@ private: void publish_mission_result(); control::BlockParamFloat _param_onboard_enabled; - - int _onboard_mission_sub; - int _offboard_mission_sub; + control::BlockParamFloat _param_loiter_radius; struct mission_s _onboard_mission; struct mission_s _offboard_mission; + int _current_onboard_mission_index; + int _current_offboard_mission_index; + struct mission_item_s _mission_item; orb_advert_t _mission_result_pub; @@ -197,6 +203,8 @@ private: MISSION_TYPE_OFFBOARD } _mission_type; + bool _loiter_set; + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c new file mode 100644 index 000000000..9696b3e53 --- /dev/null +++ b/src/modules/navigator/mission_params.c @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mission_params.c + * + * Parameters for mission. + * + * @author Julian Oes + */ + +#include + +#include + +/* + * Mission parameters, accessible via MAVLink + */ + +/** + * Loiter radius (fixed wing only) + * + * Default value of loiter radius (if not specified in mission item). + * + * @unit meters + * @min 0.0 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_LOITER_RAD, 50.0f); + +/** + * Enable onboard mission + * + * @min 0 + * @max 1 + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0); diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 2d07bc49c..e2cc475da 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -40,6 +40,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ mission.cpp \ + mission_params.c \ rtl.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 927e77391..a6ab85519 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -87,9 +87,14 @@ public: */ void load_fence_from_file(const char *filename); + /** + * Getters + */ struct vehicle_status_s* get_vstatus() { return &_vstatus; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } + int get_onboard_mission_sub() { return _onboard_mission_sub; } + int get_offboard_mission_sub() { return _offboard_mission_sub; } Geofence& get_geofence() { return _geofence; } private: @@ -105,6 +110,8 @@ private: int _params_sub; /**< notification of parameter updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ + int _onboard_mission_sub; /**< onboard mission subscription */ + int _offboard_mission_sub; /**< offboard mission subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9dd253127..b7dfec047 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -106,6 +106,8 @@ Navigator::Navigator() : _capabilities_sub(-1), _control_mode_sub(-1), _pos_sp_triplet_pub(-1), + _onboard_mission_sub(-1), + _offboard_mission_sub(-1), _vstatus({}), _control_mode({}), _global_pos({}), @@ -251,6 +253,8 @@ Navigator::task_main() _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); + _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); /* copy all topics first time */ vehicle_status_update(); -- cgit v1.2.3 From 55cf19b0826911c88ca1f62f90e4d1c7e9ddf2b2 Mon Sep 17 00:00:00 2001 From: andre-nguyen Date: Tue, 3 Jun 2014 21:45:29 -0400 Subject: added support for offboard position setpoint in mc_pos_control --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 09960d106..d870b733f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -118,6 +118,7 @@ private: int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ + int _offboard_control_pos_sp_sub; /**< offboard control position setpoint */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ @@ -132,6 +133,8 @@ private: struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + struct offboard_control_setpoint_s _offboard_control_pos_sp; /**< offboard control position setpoint + struct { param_t thr_min; @@ -277,6 +280,7 @@ MulticopterPositionControl::MulticopterPositionControl() : memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); + memset(&_offboard_control_pos_sp, 0, sizeof(_offboard_control_pos_sp)); memset(&_ref_pos, 0, sizeof(_ref_pos)); @@ -528,6 +532,7 @@ MulticopterPositionControl::task_main() _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _offboard_control_pos_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); parameters_update(true); @@ -664,6 +669,23 @@ MulticopterPositionControl::task_main() _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); } + } else if (_control_mode.flag_control_offboard_enabled) { + /* Offboard control */ + /* Team elikos doesn't really know what it is doing */ + bool updated; + orb_check(_offboard_control_pos_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), _offboard_control_pos_sp_sub, + &_offboard_control_pos_sp); + } + + /* Hopefully no one dies from this */ + pos(0) = _offboard_control_pos_sp.p1; + pos(1) = _offboard_control_pos_sp.p2; + pos(2) = _offboard_control_pos_sp.p4; + _att_sp.yaw_body = _offboard_control_pos_sp.p3; + } else { /* AUTO */ bool updated; -- cgit v1.2.3 From ce1251fcc8119b2c4e8c9a3b5e7f0e0104fc975f Mon Sep 17 00:00:00 2001 From: Benjamin O'Connell-Armand Date: Tue, 3 Jun 2014 22:30:14 -0400 Subject: Quick fix --- src/modules/mavlink/mavlink_receiver.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a2736969f..db9e06461 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -137,7 +137,7 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; - orb_advert_t _local_pos_sp_pub + orb_advert_t _local_pos_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; orb_advert_t _vicon_position_pub; -- cgit v1.2.3 From e12d3af503c5ebc105e5b3c3ef8e51bd75959329 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Tue, 3 Jun 2014 23:38:18 -0400 Subject: Added support for offboard velocity control & fixed position control flags --- src/modules/commander/commander.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 29a5f0b4a..3da43a792 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1809,12 +1809,12 @@ set_control_mode() control_mode.flag_control_offboard_enabled = true; switch (sp_offboard.mode) { - case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: - control_mode.flag_control_rates_enabled = false; + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = false; control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = true; + control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: @@ -1825,13 +1825,21 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; - case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_velocity_enabled = true; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; break; default: control_mode.flag_control_rates_enabled = false; -- cgit v1.2.3 From f1b6a3f44f3c1ade07de2a1a996b055eccd5155c Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Tue, 3 Jun 2014 23:55:21 -0400 Subject: Fixed offboard_control_pos_sp to be passed to _pos_sp vector. Added checks for position and altitude control modes. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d870b733f..b2693345c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -681,9 +681,16 @@ MulticopterPositionControl::task_main() } /* Hopefully no one dies from this */ - pos(0) = _offboard_control_pos_sp.p1; - pos(1) = _offboard_control_pos_sp.p2; - pos(2) = _offboard_control_pos_sp.p4; + /* Make sure position control is selected i.e. not only velocity control */ + if (_control_mode.flag_control_position_enabled) { + _pos_sp(0) = _offboard_control_pos_sp.p1; + _pos_sp(1) = _offboard_control_pos_sp.p2; + } + + if (_control_mode.flag_control_altitude_enabled) { + _pos_sp(2) = _offboard_control_pos_sp.p4; + } + _att_sp.yaw_body = _offboard_control_pos_sp.p3; } else { -- cgit v1.2.3 From abbf57dac655bb0832052efea9841b7e41525799 Mon Sep 17 00:00:00 2001 From: andre-nguyen Date: Wed, 4 Jun 2014 00:04:31 -0400 Subject: had the wrong variable and wrong setpoint type --- src/modules/mavlink/mavlink_receiver.cpp | 1 + src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 +++++++------- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 78c50c422..2157ae00b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -379,6 +379,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message case 4: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + ml_armed = true; break; default: break; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d870b733f..4d0c3216a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -133,7 +133,7 @@ private: struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ - struct offboard_control_setpoint_s _offboard_control_pos_sp; /**< offboard control position setpoint + struct vehicle_local_position_setpoint_s _offboard_control_pos_sp; /**< offboard control position setpoint */ struct { @@ -532,7 +532,7 @@ MulticopterPositionControl::task_main() _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _offboard_control_pos_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + _offboard_control_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); parameters_update(true); @@ -676,15 +676,15 @@ MulticopterPositionControl::task_main() orb_check(_offboard_control_pos_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), _offboard_control_pos_sp_sub, + orb_copy(ORB_ID(vehicle_local_position_setpoint), _offboard_control_pos_sp_sub, &_offboard_control_pos_sp); } /* Hopefully no one dies from this */ - pos(0) = _offboard_control_pos_sp.p1; - pos(1) = _offboard_control_pos_sp.p2; - pos(2) = _offboard_control_pos_sp.p4; - _att_sp.yaw_body = _offboard_control_pos_sp.p3; + _pos_sp(0) = _offboard_control_pos_sp.x; + _pos_sp(1) = _offboard_control_pos_sp.y; + _pos_sp(2) = _offboard_control_pos_sp.z; + _att_sp.yaw_body = _offboard_control_pos_sp.yaw; } else { /* AUTO */ -- cgit v1.2.3 From af56879fdd94c1b712657e071324b3a6f147caa7 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Wed, 4 Jun 2014 02:32:44 -0400 Subject: Removed duplicate _pos_sp assignation --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index b73becf3c..87a8385d3 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -680,12 +680,6 @@ MulticopterPositionControl::task_main() &_offboard_control_pos_sp); } - /* Hopefully no one dies from this */ - _pos_sp(0) = _offboard_control_pos_sp.x; - _pos_sp(1) = _offboard_control_pos_sp.y; - _pos_sp(2) = _offboard_control_pos_sp.z; - _att_sp.yaw_body = _offboard_control_pos_sp.yaw; - /* Make sure position control is selected i.e. not only velocity control */ if (_control_mode.flag_control_position_enabled) { _pos_sp(0) = _offboard_control_pos_sp.x; -- cgit v1.2.3 From 7a6d33ba47ccc65354427518674531e84e28f54c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Jun 2014 09:51:14 +0200 Subject: Extend estimator status --- src/modules/uORB/topics/estimator_status.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h index 5530cdb21..7f26b505b 100644 --- a/src/modules/uORB/topics/estimator_status.h +++ b/src/modules/uORB/topics/estimator_status.h @@ -64,9 +64,9 @@ struct estimator_status_report { uint64_t timestamp; /**< Timestamp in microseconds since boot */ float states[32]; /**< Internal filter states */ float n_states; /**< Number of states effectively used */ - bool states_nan; /**< If set to true, one of the states is NaN */ - bool covariance_nan; /**< If set to true, the covariance matrix went NaN */ - bool kalman_gain_nan; /**< If set to true, the Kalman gain matrix went NaN */ + uint8_t nan_flags; /**< Bitmask to indicate NaN states */ + uint8_t health_flags; /**< Bitmask to indicate sensor health states (vel, pos, hgt) */ + uint8_t timeout_flags; /**< Bitmask to indicate timeout flags (vel, pos, hgt) */ }; -- cgit v1.2.3 From 77a4711ff992b4cbda03f539dd2e7bd927b76e95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Jun 2014 09:51:40 +0200 Subject: Log extended estimator status --- src/modules/sdlog2/sdlog2.c | 27 +++++++++++++++++---------- src/modules/sdlog2/sdlog2_messages.h | 29 +++++++++++++++++++---------- 2 files changed, 36 insertions(+), 20 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c19579f0f..5e6946658 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -976,7 +976,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; struct log_TELE_s log_TELE; - struct log_ESTM_s log_ESTM; + struct log_EST0_s log_EST0; + struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; struct log_GS0A_s log_GS0A; @@ -1489,15 +1490,21 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { - log_msg.msg_type = LOG_ESTM_MSG; - unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s); - memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s)); - memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy); - log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states; - log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan; - log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan; - log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan; - LOGBUFFER_WRITE_AND_COUNT(ESTM); + log_msg.msg_type = LOG_EST0_MSG; + unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s); + memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s)); + memcpy(&(log_msg.body.log_EST0.s), buf.estimator_status.states, maxcopy0); + log_msg.body.log_EST0.n_states = buf.estimator_status.n_states; + log_msg.body.log_EST0.nan_flags = buf.estimator_status.nan_flags; + log_msg.body.log_EST0.health_flags = buf.estimator_status.health_flags; + log_msg.body.log_EST0.timeout_flags = buf.estimator_status.timeout_flags; + LOGBUFFER_WRITE_AND_COUNT(EST0); + + log_msg.msg_type = LOG_EST1_MSG; + unsigned maxcopy1 = ((sizeof(buf.estimator_status.states) - maxcopy0) < sizeof(log_msg.body.log_EST1.s)) ? (sizeof(buf.estimator_status.states) - maxcopy0) : sizeof(log_msg.body.log_EST1.s); + memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s)); + memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1); + LOGBUFFER_WRITE_AND_COUNT(EST1); } /* --- TECS STATUS --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a874351b3..a3c9d2440 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -288,15 +288,7 @@ struct log_TELE_s { uint8_t txbuf; }; -/* --- ESTM - ESTIMATOR STATUS --- */ -#define LOG_ESTM_MSG 23 -struct log_ESTM_s { - float s[10]; - uint8_t n_states; - uint8_t states_nan; - uint8_t covariance_nan; - uint8_t kalman_gain_nan; -}; +// ID 23 available /* --- PWR - ONBOARD POWER SYSTEM --- */ #define LOG_PWR_MSG 24 @@ -375,6 +367,22 @@ struct log_WIND_s { float cov_y; }; +/* --- EST0 - ESTIMATOR STATUS --- */ +#define LOG_EST0_MSG 32 +struct log_EST0_s { + float s[12]; + uint8_t n_states; + uint8_t nan_flags; + uint8_t health_flags; + uint8_t timeout_flags; +}; + +/* --- EST1 - ESTIMATOR STATUS --- */ +#define LOG_EST1_MSG 33 +struct log_EST1_s { + float s[16]; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -423,7 +431,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), - LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"), + LOG_FORMAT(EST0, "ffffffffffffffBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,nStat,flagsNaN"), + LOG_FORMAT(EST1, "ffffffffffffffff", "s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), -- cgit v1.2.3 From cf67e810a4e21b97a12862fd55572b5d025156b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Jun 2014 09:52:21 +0200 Subject: More detailed estimator status feedback --- src/modules/ekf_att_pos_estimator/estimator.cpp | 10 +++++----- src/modules/ekf_att_pos_estimator/estimator.h | 7 ++++++- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index bfb2007af..5ac6b079f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2283,21 +2283,21 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { // check all integrators if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { - err_report->statesNaN = true; + err_report->angNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { - err_report->statesNaN = true; + err_report->angNaN = true; ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { - err_report->statesNaN = true; + err_report->summedDelVelNaN = true; ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); err = true; goto out; @@ -2308,7 +2308,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { for (unsigned j = 0; j < n_states; j++) { if (!isfinite(KH[i][j])) { - err_report->covarianceNaN = true; + err_report->KHNaN = true; err = true; ekf_debug("KH NaN"); goto out; @@ -2316,7 +2316,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(KHP[i][j])) { - err_report->covarianceNaN = true; + err_report->KHPNaN = true; err = true; ekf_debug("KHP NaN"); goto out; diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index ec82896fb..401462923 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -66,9 +66,14 @@ struct ekf_status_report { uint32_t posFailTime; uint32_t hgtFailTime; float states[n_states]; - bool statesNaN; + bool angNaN; + bool summedDelVelNaN; + bool KHNaN; + bool KHPNaN; + bool PNaN; bool covarianceNaN; bool kalmanGainsNaN; + bool statesNaN; }; class AttPosEKF { -- cgit v1.2.3 From 19154f29d822afdc0a33bf3be55fab63b32f23c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Jun 2014 09:52:34 +0200 Subject: Copy estimator status updates to system status logging --- .../ekf_att_pos_estimator_main.cpp | 98 ++++++++++++++++------ 1 file changed, 71 insertions(+), 27 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 0a5b26f4a..1e2582952 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -135,6 +135,11 @@ public: */ int trip_nan(); + /** + * Enable logging. + */ + int enable_logging(bool enable); + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -202,6 +207,7 @@ private: bool _gyro_valid; bool _accel_valid; bool _mag_valid; + bool _ekf_logging; ///< log EKF state int _mavlink_fd; @@ -356,6 +362,7 @@ FixedwingEstimator::FixedwingEstimator() : _gyro_valid(false), _accel_valid(false), _mag_valid(false), + _ekf_logging(true), _mavlink_fd(-1), _ekf(nullptr) { @@ -447,6 +454,14 @@ FixedwingEstimator::~FixedwingEstimator() estimator::g_estimator = nullptr; } +int +FixedwingEstimator::enable_logging(bool logging) +{ + _ekf_logging = logging; + + return 0; +} + int FixedwingEstimator::parameters_update() { @@ -992,20 +1007,57 @@ FixedwingEstimator::task_main() warnx("NUMERIC ERROR IN FILTER"); } + struct estimator_status_report rep; + memset(&rep, 0, sizeof(rep)); + + struct ekf_status_report ekf_report; + // If non-zero, we got a filter reset if (check > 0 && check != 3) { - struct ekf_status_report ekf_report; - _ekf->GetLastErrorState(&ekf_report); - struct estimator_status_report rep; - memset(&rep, 0, sizeof(rep)); + /* set sensors to de-initialized state */ + _gyro_valid = false; + _accel_valid = false; + _mag_valid = false; + + _baro_init = false; + _gps_initialized = false; + _initialized = false; + last_sensor_timestamp = hrt_absolute_time(); + last_run = last_sensor_timestamp; + + _ekf->ZeroVariables(); + _ekf->statesInitialised = false; + _ekf->dtIMU = 0.01f; + + // Let the system re-initialize itself + continue; + + } else if (_ekf_logging) { + _ekf->GetFilterState(&ekf_report); + } + + if (_ekf_logging || check) { rep.timestamp = hrt_absolute_time(); - rep.states_nan = ekf_report.statesNaN; - rep.covariance_nan = ekf_report.covarianceNaN; - rep.kalman_gain_nan = ekf_report.kalmanGainsNaN; + rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0); + rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1); + rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2); + rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3); + rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4); + rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5); + rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6); + rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7); + + rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); + rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); + rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); + + rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); + rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); + rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); // Copy all states or at least all that we can fit unsigned i = 0; @@ -1024,25 +1076,6 @@ FixedwingEstimator::task_main() } else { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); } - - /* set sensors to de-initialized state */ - _gyro_valid = false; - _accel_valid = false; - _mag_valid = false; - - _baro_init = false; - _gps_initialized = false; - _initialized = false; - last_sensor_timestamp = hrt_absolute_time(); - last_run = last_sensor_timestamp; - - _ekf->ZeroVariables(); - _ekf->statesInitialised = false; - _ekf->dtIMU = 0.01f; - - // Let the system re-initialize itself - continue; - } @@ -1503,7 +1536,7 @@ int FixedwingEstimator::trip_nan() { int ekf_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 1) - errx(1, "usage: ekf_att_pos_estimator {start|stop|status}"); + errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}"); if (!strcmp(argv[1], "start")) { @@ -1557,6 +1590,17 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) } } + if (!strcmp(argv[1], "logging")) { + if (estimator::g_estimator) { + int ret = estimator::g_estimator->enable_logging(true); + + exit(ret); + + } else { + errx(1, "not running"); + } + } + warnx("unrecognized command"); return 1; } -- cgit v1.2.3 From 5aef22310e0dbec9f758e4cf1df5ad93cbd989ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Jun 2014 10:31:23 +0200 Subject: Ensure states are actually copied in non-error mode --- .../ekf_att_pos_estimator_main.cpp | 37 ++++++++++------------ src/modules/ekf_att_pos_estimator/estimator.cpp | 6 ++++ 2 files changed, 23 insertions(+), 20 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 1e2582952..0fea362c5 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1042,33 +1042,30 @@ FixedwingEstimator::task_main() if (_ekf_logging || check) { rep.timestamp = hrt_absolute_time(); - rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0); - rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1); - rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2); - rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3); - rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4); - rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5); - rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6); - rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7); - - rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); - rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); - rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); - - rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); - rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); - rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); + rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0); + rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1); + rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2); + rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3); + rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4); + rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5); + rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6); + rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7); + + rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); + rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); + rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); + + rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); + rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); + rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); // Copy all states or at least all that we can fit - unsigned i = 0; unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; - while ((i < ekf_n_states) && (i < max_states)) { - + for (unsigned i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; - i++; } if (_estimator_status_pub > 0) { diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5ac6b079f..89e137adc 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2610,6 +2610,12 @@ void AttPosEKF::ZeroVariables() void AttPosEKF::GetFilterState(struct ekf_status_report *state) { + + // Copy states + for (unsigned i = 0; i < n_states; i++) { + current_ekf_state.states[i] = states[i]; + } + memcpy(state, ¤t_ekf_state, sizeof(*state)); } -- cgit v1.2.3 From fffd3c5d1242dd908b784ece996513b034e98709 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Jun 2014 10:32:46 +0200 Subject: Fix format specifier --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a3c9d2440..81bbbe560 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -431,7 +431,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), - LOG_FORMAT(EST0, "ffffffffffffffBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,nStat,flagsNaN"), + LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), LOG_FORMAT(EST1, "ffffffffffffffff", "s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), -- cgit v1.2.3 From 72b70f19ebf7de3878a3240e23dc88a90938365a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Jun 2014 10:33:15 +0200 Subject: Always log, also in HIL. There are enough mishaps only visible in logs that we actually want to spend the microSD space for those --- ROMFS/px4fmu_common/init.d/rcS | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6d06f897a..f70e0ed77 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -428,11 +428,10 @@ then # sh /etc/init.d/rc.sensors - if [ $HIL == no ] - then - echo "[init] Start logging" - sh /etc/init.d/rc.logging - fi + # + # Start logging in all modes, including HIL + # + sh /etc/init.d/rc.logging if [ $GPS == yes ] then -- cgit v1.2.3 From 09e4cc605ba898c8fc2d92c105ec511ffcb6781d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Jun 2014 15:16:20 +0200 Subject: navigator: use different param names for mission and RTL --- src/modules/navigator/mission.cpp | 4 ++-- src/modules/navigator/mission.h | 2 +- src/modules/navigator/navigator_main.cpp | 4 ++-- src/modules/navigator/rtl.cpp | 4 ++-- src/modules/navigator/rtl.h | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 38badcff9..879e5b618 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -57,8 +57,8 @@ #include "mission.h" -Mission::Mission(Navigator *navigator) : - SuperBlock(NULL, "MIS"), +Mission::Mission(Navigator *navigator, const char *name) : + SuperBlock(NULL, name), _navigator(navigator), _first_run(true), _param_onboard_enabled(this, "ONBOARD_EN"), diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index f75c8a882..a9ef71606 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -64,7 +64,7 @@ public: /** * Constructor */ - Mission(Navigator *navigator); + Mission(Navigator *navigator, const char *name); /** * Destructor diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b7dfec047..e166058b9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -121,9 +121,9 @@ Navigator::Navigator() : _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), - _mission(this), + _mission(this, "MIS"), //_loiter(&_global_pos, &_home_pos, &_vstatus), - _rtl(this), + _rtl(this, "RTL"), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 9d7886aa6..10499085b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -51,8 +51,8 @@ #include "rtl.h" -RTL::RTL(Navigator *navigator) : - Mission(navigator), +RTL::RTL(Navigator *navigator, const char *name) : + Mission(navigator, name), _mavlink_fd(-1), _rtl_state(RTL_STATE_NONE), _home_position({}), diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 60336d5d0..a3a819403 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -58,7 +58,7 @@ public: /** * Constructor */ - RTL(Navigator *navigator); + RTL(Navigator *navigator, const char *name); /** * Destructor -- cgit v1.2.3 From 425b454a87f0eb4dd0300154cdeffa5723c1b3b8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Jun 2014 15:33:09 +0200 Subject: navigator: parameter cleanup --- src/modules/navigator/mission_params.c | 15 ++++- src/modules/navigator/module.mk | 2 +- src/modules/navigator/navigator.h | 20 ------- src/modules/navigator/navigator_main.cpp | 60 ++++---------------- src/modules/navigator/navigator_params.c | 96 -------------------------------- src/modules/navigator/rtl.cpp | 1 + src/modules/navigator/rtl.h | 1 + src/modules/navigator/rtl_params.c | 11 ++++ 8 files changed, 39 insertions(+), 167 deletions(-) delete mode 100644 src/modules/navigator/navigator_params.c diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 9696b3e53..f5067a70b 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -48,9 +48,20 @@ */ /** - * Loiter radius (fixed wing only) + * Take-off altitude * - * Default value of loiter radius (if not specified in mission item). + * Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to + * MIS_TAKEOFF_ALT on takeoff, then go to waypoint. + * + * @unit meters + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); + +/** + * Loiter radius after/during mission (FW only) + * + * Default value of loiter radius (fixedwing only). * * @unit meters * @min 0.0 diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index e2cc475da..88d8aac5d 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -38,10 +38,10 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ - navigator_params.c \ mission.cpp \ mission_params.c \ rtl.cpp \ + rtl_params.c \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index a6ab85519..9f877cd76 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -107,7 +107,6 @@ private: int _global_pos_sub; /**< global position subscription */ int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ @@ -143,25 +142,6 @@ private: bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ - struct { - float acceptance_radius; - float loiter_radius; - int onboard_mission_enabled; - float takeoff_alt; - } _parameters; /**< local copies of parameters */ - - struct { - param_t acceptance_radius; - param_t loiter_radius; - param_t onboard_mission_enabled; - param_t takeoff_alt; - } _parameter_handles; /**< handles for parameters */ - - /** - * Update our local parameter cache. - */ - void parameters_update(); - /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e166058b9..3c8875a74 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -64,12 +64,10 @@ #include #include #include -#include #include #include #include -#include #include #include #include @@ -102,7 +100,6 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), - _params_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), _pos_sp_triplet_pub(-1), @@ -127,14 +124,8 @@ Navigator::Navigator() : _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _update_triplet(false), - _parameters({}), - _parameter_handles({}) + _update_triplet(false) { - _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); - _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); - _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); - _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); } Navigator::~Navigator() @@ -162,23 +153,6 @@ Navigator::~Navigator() navigator::g_navigator = nullptr; } -void -Navigator::parameters_update() -{ - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); - param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); - param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); - param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - - //_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); - - _geofence.updateParams(); -} - void Navigator::global_position_update() { @@ -251,7 +225,6 @@ Navigator::task_main() _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); @@ -259,7 +232,6 @@ Navigator::task_main() /* copy all topics first time */ vehicle_status_update(); vehicle_control_mode_update(); - parameters_update(); global_position_update(); home_position_update(); navigation_capabilities_update(); @@ -271,21 +243,19 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[6]; + struct pollfd fds[5]; /* Setup of loop */ - fds[0].fd = _params_sub; + fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; - fds[1].fd = _global_pos_sub; + fds[1].fd = _home_pos_sub; fds[1].events = POLLIN; - fds[2].fd = _home_pos_sub; + fds[2].fd = _capabilities_sub; fds[2].events = POLLIN; - fds[3].fd = _capabilities_sub; + fds[3].fd = _vstatus_sub; fds[3].events = POLLIN; - fds[4].fd = _vstatus_sub; + fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; - fds[5].fd = _control_mode_sub; - fds[5].events = POLLIN; while (!_task_should_exit) { @@ -311,33 +281,27 @@ Navigator::task_main() } /* vehicle control mode updated */ - if (fds[5].revents & POLLIN) { + if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ - if (fds[4].revents & POLLIN) { + if (fds[3].revents & POLLIN) { vehicle_status_update(); } - /* parameters updated */ - if (fds[0].revents & POLLIN) { - parameters_update(); - /* note that these new parameters won't be in effect until a mission triplet is published again */ - } - /* navigation capabilities updated */ - if (fds[3].revents & POLLIN) { + if (fds[2].revents & POLLIN) { navigation_capabilities_update(); } /* home position updated */ - if (fds[2].revents & POLLIN) { + if (fds[1].revents & POLLIN) { home_position_update(); } /* global position updated */ - if (fds[1].revents & POLLIN) { + if (fds[0].revents & POLLIN) { global_position_update(); /* Check geofence violation */ diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c deleted file mode 100644 index ad079a250..000000000 --- a/src/modules/navigator/navigator_params.c +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file navigator_params.c - * - * Parameters defined by the navigator task. - * - * @author Lorenz Meier - * @author Julian Oes - * @author Anton Babushkin - */ - -#include - -#include - -/* - * Navigator parameters, accessible via MAVLink - */ - -/** - * Waypoint acceptance radius - * - * Default value of acceptance radius (if not specified in mission item). - * - * @unit meters - * @min 0.0 - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); - -/** - * Loiter radius (fixed wing only) - * - * Default value of loiter radius (if not specified in mission item). - * - * @unit meters - * @min 0.0 - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); - -/** - * Enable onboard mission - * - * @group Navigation - */ -PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); - -/** - * Take-off altitude - * - * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); - -/** - * Enable parachute deployment - * - * @group Navigation - */ -PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 10499085b..d4e609584 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -58,6 +58,7 @@ RTL::RTL(Navigator *navigator, const char *name) : _home_position({}), _loiter_radius(50), _acceptance_radius(50), + _param_loiter_rad(this, "LOITER_RAD"), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), _param_land_delay(this, "LAND_DELAY") diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index a3a819403..d84fd1a6f 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -92,6 +92,7 @@ private: float _loiter_radius; float _acceptance_radius; + control::BlockParamFloat _param_loiter_rad; control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 7a8b1d745..bfe6ce7e1 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -47,6 +47,17 @@ * RTL parameters, accessible via MAVLink */ +/** + * Loiter radius after RTL (FW only) + * + * Default value of loiter radius after RTL (fixedwing only). + * + * @unit meters + * @min 0.0 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); + /** * RTL altitude * -- cgit v1.2.3 From e03411fc89fcdf36bfca68b5e27e319b56985782 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 4 Jun 2014 18:43:30 +0200 Subject: mtecs: more comments for params --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 591257611..39514b223 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -58,7 +58,8 @@ PARAM_DEFINE_INT32(MT_ENABLED, 1); /** - * Total Energy Rate Control FF + * Total Energy Rate Control Feedforward + * Maps the total energy rate setpoint to the throttle setpoint * * @min 0.0 * @max 10.0 @@ -68,6 +69,7 @@ PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f); /** * Total Energy Rate Control P + * Maps the total energy rate error to the throttle setpoint * * @min 0.0 * @max 10.0 @@ -77,6 +79,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f); /** * Total Energy Rate Control I + * Maps the integrated total energy rate to the throttle setpoint * * @min 0.0 * @max 10.0 @@ -85,7 +88,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f); PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f); /** - * Total Energy Rate Control OFF (Cruise throttle) + * Total Energy Rate Control Offset (Cruise throttle sp) * * @min 0.0 * @max 10.0 @@ -94,7 +97,8 @@ PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f); PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f); /** - * Energy Distribution Rate Control FF + * Energy Distribution Rate Control Feedforward + * Maps the energy distribution rate setpoint to the pitch setpoint * * @min 0.0 * @max 10.0 @@ -104,6 +108,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.1f); /** * Energy Distribution Rate Control P + * Maps the energy distribution rate error to the pitch setpoint * * @min 0.0 * @max 10.0 @@ -113,6 +118,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f); /** * Energy Distribution Rate Control I + * Maps the integrated energy distribution rate error to the pitch setpoint * * @min 0.0 * @max 10.0 @@ -122,7 +128,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f); /** - * Total Energy Distribution OFF (Cruise pitch sp) + * Total Energy Distribution Offset (Cruise pitch sp) * * @min 0.0 * @max 10.0 @@ -170,6 +176,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); /** * P gain for the altitude control + * Maps the altitude error to the flight path angle setpoint * * @min 0.0f * @max 10.0f @@ -179,6 +186,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f); /** * D gain for the altitude control + * Maps the change of altitude error to the flight path angle setpoint * * @min 0.0f * @max 10.0f @@ -187,7 +195,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f); PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f); /** - * Lowpass for FPA error derivative (see MT_FPA_D) + * Lowpass for FPA error derivative calculation (see MT_FPA_D) * * @group mTECS */ @@ -217,6 +225,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); /** * P gain for the airspeed control + * Maps the airspeed error to the acceleration setpoint * * @min 0.0f * @max 10.0f @@ -241,7 +250,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); /** - * Airspeed derivative lowpass + * Airspeed derivative calculation lowpass * * @group mTECS */ -- cgit v1.2.3 From ba36890ef32efd6b51490237f12378dfe1738353 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Jun 2014 16:59:18 +0200 Subject: sdlog2: Fix estimator state labels --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 81bbbe560..9a26e1c51 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -432,7 +432,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), - LOG_FORMAT(EST1, "ffffffffffffffff", "s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29"), + LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), -- cgit v1.2.3 From 241837c9df04faaf4b741875cf6b47a22819f010 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Jun 2014 17:03:31 +0200 Subject: change numbers in printing routine to match 0-based indices --- .../ekf_att_pos_estimator_main.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 0fea362c5..ee3c60e63 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1459,15 +1459,15 @@ FixedwingEstimator::print_status() printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); - printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); - printf("states (accel offs) [14]: %8.4f\n", (double)_ekf->states[13]); - printf("states (wind) [15-16]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); - printf("states (earth mag) [17-19]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); - printf("states (body mag) [20-22]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); - printf("states (terrain) [23]: %8.4f\n", (double)_ekf->states[22]); + printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); + printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); + printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); + printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); + printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); printf("states: %s %s %s %s %s %s %s %s %s %s\n", (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", -- cgit v1.2.3 From 7af1103bf3d4936c259e4ee44454d7e34100a7d0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 00:55:18 +0200 Subject: navigator: mission and loiter working now --- src/modules/navigator/loiter.cpp | 79 ++++++++++++++++++++++++++++++++ src/modules/navigator/loiter.h | 69 ++++++++++++++++++++++++++++ src/modules/navigator/mission.cpp | 33 +++++++------ src/modules/navigator/mission.h | 20 ++++---- src/modules/navigator/module.mk | 1 + src/modules/navigator/navigator.h | 28 ++++++----- src/modules/navigator/navigator_main.cpp | 19 ++++---- 7 files changed, 202 insertions(+), 47 deletions(-) create mode 100644 src/modules/navigator/loiter.cpp create mode 100644 src/modules/navigator/loiter.h diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp new file mode 100644 index 000000000..6da08f072 --- /dev/null +++ b/src/modules/navigator/loiter.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file loiter.cpp + * + * Helper class to loiter + * + * @author Julian Oes + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "loiter.h" + +Loiter::Loiter(Navigator *navigator, const char *name) : + Mission(navigator, name) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + reset(); +} + +Loiter::~Loiter() +{ +} + +bool +Loiter::update(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + /* set loiter item, don't reuse an existing position setpoint */ + return set_loiter_item(false, pos_sp_triplet);; +} + +void +Loiter::reset() +{ +} + diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h new file mode 100644 index 000000000..4ae265e44 --- /dev/null +++ b/src/modules/navigator/loiter.h @@ -0,0 +1,69 @@ +/*************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file loiter.h + * + * Helper class to loiter + * + * @author Julian Oes + */ + +#ifndef NAVIGATOR_LOITER_H +#define NAVIGATOR_LOITER_H + +#include +#include + +#include "mission.h" + +class Navigator; + +class Loiter : public Mission +{ +public: + /** + * Constructor + */ + Loiter(Navigator *navigator, const char *name); + + /** + * Destructor + */ + virtual ~Loiter(); + + virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + + virtual void reset(); +}; + +#endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 879e5b618..839c4c960 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -70,8 +70,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_item({0}), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE), - _loiter_set(false) + _mission_type(MISSION_TYPE_NONE) { /* load initial params */ updateParams(); @@ -88,13 +87,11 @@ void Mission::reset() { _first_run = true; - _loiter_set = false; } bool Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { - /* check if anything has changed */ bool onboard_updated = is_onboard_mission_updated(); bool offboard_updated = is_offboard_mission_updated(); @@ -115,13 +112,6 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) updated = true; } - /* maybe we couldn't actually set a mission, therefore lets set a loiter setpoint */ - if (_mission_type == MISSION_TYPE_NONE && !_loiter_set) { - bool use_current_pos_sp = (pos_sp_triplet->current.valid && _waypoint_position_reached); - set_loiter_item(use_current_pos_sp, pos_sp_triplet); - updated = true; - _loiter_set = true; - } return updated; } @@ -243,12 +233,18 @@ Mission::mission_item_to_position_setpoint(const struct mission_item_s *item, st } } -void +bool Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet) { + if (_navigator->get_is_in_loiter()) { + /* already loitering, bail out */ + return false; + } + if (reuse_current_pos_sp && pos_sp_triplet->current.valid) { - /* nothing to be done, just use the current item */ + /* leave position setpoint as is */ } else { + /* use current position */ pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; @@ -261,6 +257,9 @@ Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_tri pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = true; pos_sp_triplet->next.valid = false; + + _navigator->set_is_in_loiter(true); + return true; } @@ -372,14 +371,18 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { /* try setting onboard mission item */ _mission_type = MISSION_TYPE_ONBOARD; - _loiter_set = false; + _navigator->set_is_in_loiter(false); } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { /* try setting offboard mission item */ _mission_type = MISSION_TYPE_OFFBOARD; - _loiter_set = false; + _navigator->set_is_in_loiter(false); } else { _mission_type = MISSION_TYPE_NONE; + + bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached; + reset_mission_item_reached(); + set_loiter_item(use_current_pos_sp, pos_sp_triplet); } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index a9ef71606..4d0083d85 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -32,6 +32,7 @@ ****************************************************************************/ /** * @file mission.h + * * Helper class to access missions * * @author Julian Oes @@ -58,7 +59,7 @@ class Navigator; -class __EXPORT Mission : public control::SuperBlock +class Mission : public control::SuperBlock { public: /** @@ -99,17 +100,12 @@ protected: /** * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position + * @return true if setpoint has changed */ - void set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet); + bool set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet); class Navigator *_navigator; - bool _waypoint_position_reached; - bool _waypoint_yaw_reached; - hrt_abstime _time_first_inside_orbit; - - bool _first_run; - private: /** * Update onboard mission topic @@ -183,6 +179,12 @@ private: */ void publish_mission_result(); + bool _waypoint_position_reached; + bool _waypoint_yaw_reached; + hrt_abstime _time_first_inside_orbit; + + bool _first_run; + control::BlockParamFloat _param_onboard_enabled; control::BlockParamFloat _param_loiter_radius; @@ -203,8 +205,6 @@ private: MISSION_TYPE_OFFBOARD } _mission_type; - bool _loiter_set; - MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 88d8aac5d..f4243c5b8 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -40,6 +40,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ mission.cpp \ mission_params.c \ + loiter.cpp \ rtl.cpp \ rtl_params.c \ mission_feasibility_checker.cpp \ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 9f877cd76..0c551bb41 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -49,6 +49,7 @@ #include #include "mission.h" +#include "loiter.h" #include "rtl.h" #include "geofence.h" @@ -87,15 +88,21 @@ public: */ void load_fence_from_file(const char *filename); + /** + * Setters + */ + void set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; } + /** * Getters */ - struct vehicle_status_s* get_vstatus() { return &_vstatus; } - struct vehicle_global_position_s* get_global_position() { return &_global_pos; } - struct home_position_s* get_home_position() { return &_home_pos; } - int get_onboard_mission_sub() { return _onboard_mission_sub; } - int get_offboard_mission_sub() { return _offboard_mission_sub; } - Geofence& get_geofence() { return _geofence; } + struct vehicle_status_s* get_vstatus() { return &_vstatus; } + struct vehicle_global_position_s* get_global_position() { return &_global_pos; } + struct home_position_s* get_home_position() { return &_home_pos; } + int get_onboard_mission_sub() { return _onboard_mission_sub; } + int get_offboard_mission_sub() { return _offboard_mission_sub; } + Geofence& get_geofence() { return _geofence; } + bool get_is_in_loiter() { return _is_in_loiter; } private: @@ -133,14 +140,11 @@ private: bool _inside_fence; /**< vehicle is inside fence */ Mission _mission; /**< class that handles the missions */ - //Loiter _loiter; /**< class that handles loiter */ + Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ - bool _waypoint_position_reached; /**< flags if the position of the mission item has been reached */ - bool _waypoint_yaw_reached; /**< flags if the yaw position of the mission item has been reached */ - uint64_t _time_first_inside_orbit; /**< the time when we were first in the acceptance radius of the mission item */ - - bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ + bool _is_in_loiter; /**< flags if current position SP can be used to loiter */ + bool _update_triplet; /**< flags if position SP triplet needs to be published */ /** * Retrieve global position diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3c8875a74..44c1075c1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -102,9 +102,9 @@ Navigator::Navigator() : _vstatus_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), - _pos_sp_triplet_pub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), + _pos_sp_triplet_pub(-1), _vstatus({}), _control_mode({}), _global_pos({}), @@ -119,11 +119,8 @@ Navigator::Navigator() : _fence_valid(false), _inside_fence(true), _mission(this, "MIS"), - //_loiter(&_global_pos, &_home_pos, &_vstatus), + _loiter(this, "LOI"), _rtl(this, "RTL"), - _waypoint_position_reached(false), - _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), _update_triplet(false) { } @@ -325,26 +322,29 @@ Navigator::task_main() case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: _mission.reset(); + _loiter.reset(); _rtl.reset(); + _is_in_loiter = false; break; case NAVIGATION_STATE_AUTO_MISSION: _update_triplet = _mission.update(&_pos_sp_triplet); - _rtl.reset(); break; case NAVIGATION_STATE_AUTO_LOITER: - //_loiter.update(); + _update_triplet = _loiter.update(&_pos_sp_triplet); break; case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTL_RC: case NAVIGATION_STATE_AUTO_RTL_DL: - _mission.reset(); _update_triplet = _rtl.update(&_pos_sp_triplet); break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: default: + _mission.reset(); + _loiter.reset(); + _rtl.reset(); + _is_in_loiter = false; break; - } if (_update_triplet ) { @@ -354,7 +354,6 @@ Navigator::task_main() perf_end(_loop_perf); } - warnx("exiting."); _navigator_task = -1; -- cgit v1.2.3 From 034856a24c4ffd41c991797e75517ed456f77a54 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 17:16:49 +0200 Subject: mission_feasability_checker: fixed warnings --- src/modules/navigator/mission_feasibility_checker.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index e1a6854b2..dd7f4c801 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -215,11 +215,12 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size // float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; + return false; } void MissionFeasibilityChecker::updateNavigationCapabilities() { - int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } void MissionFeasibilityChecker::init() -- cgit v1.2.3 From 9bfae10b73406ca4f6600a0441c6edf5077f1446 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 17:17:06 +0200 Subject: geofence: fixed warnings --- src/modules/navigator/geofence.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index bc8dbca50..266215308 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -78,7 +78,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { double lat = vehicle->lat / 1e7d; double lon = vehicle->lon / 1e7d; - float alt = vehicle->alt; + //float alt = vehicle->alt; return inside(lat, lon, vehicle->alt); } @@ -116,9 +116,9 @@ bool Geofence::inside(double lat, double lon, float altitude) } // skip vertex 0 (return point) - if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) && - (lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) / - (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) { + if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && + (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / + (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { c = !c; } @@ -294,4 +294,5 @@ Geofence::loadFromFile(const char *filename) int Geofence::clearDm() { dm_clear(DM_KEY_FENCE_POINTS); + return OK; } -- cgit v1.2.3 From d78c3a224267f4dbd1fac72e893c81b83b43df9b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 17:17:41 +0200 Subject: navigator: new class structure, loiter and mission working --- src/modules/navigator/loiter.cpp | 3 +- src/modules/navigator/loiter.h | 19 ++- src/modules/navigator/mission.cpp | 194 +++------------------------- src/modules/navigator/mission.h | 46 +------ src/modules/navigator/mission_block.cpp | 215 +++++++++++++++++++++++++++++++ src/modules/navigator/mission_block.h | 105 +++++++++++++++ src/modules/navigator/module.mk | 2 + src/modules/navigator/navigator.h | 4 + src/modules/navigator/navigator_main.cpp | 34 +++-- src/modules/navigator/navigator_mode.cpp | 70 ++++++++++ src/modules/navigator/navigator_mode.h | 86 +++++++++++++ src/modules/navigator/rtl.cpp | 4 +- src/modules/navigator/rtl.h | 24 ++-- 13 files changed, 564 insertions(+), 242 deletions(-) create mode 100644 src/modules/navigator/mission_block.cpp create mode 100644 src/modules/navigator/mission_block.h create mode 100644 src/modules/navigator/navigator_mode.cpp create mode 100644 src/modules/navigator/navigator_mode.h diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 6da08f072..035d035e1 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -53,7 +53,8 @@ #include "loiter.h" Loiter::Loiter(Navigator *navigator, const char *name) : - Mission(navigator, name) + NavigatorMode(navigator, name), + MissionBlock(navigator) { /* load initial params */ updateParams(); diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 4ae265e44..a83b53f43 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -44,11 +44,10 @@ #include #include -#include "mission.h" +#include "navigator_mode.h" +#include "mission_block.h" -class Navigator; - -class Loiter : public Mission +class Loiter : public NavigatorMode, MissionBlock { public: /** @@ -59,11 +58,17 @@ public: /** * Destructor */ - virtual ~Loiter(); + ~Loiter(); - virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + /** + * This function is called while the mode is inactive + */ + bool update(struct position_setpoint_triplet_s *pos_sp_triplet); - virtual void reset(); + /** + * This function is called while the mode is active + */ + void reset(); }; #endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 839c4c960..167f24ca6 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -56,18 +56,15 @@ #include "navigator.h" #include "mission.h" - Mission::Mission(Navigator *navigator, const char *name) : - SuperBlock(NULL, name), - _navigator(navigator), - _first_run(true), + NavigatorMode(navigator, name), + MissionBlock(navigator), _param_onboard_enabled(this, "ONBOARD_EN"), _param_loiter_radius(this, "LOITER_RAD"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), - _mission_item({0}), _mission_result_pub(-1), _mission_result({0}), _mission_type(MISSION_TYPE_NONE) @@ -92,14 +89,23 @@ Mission::reset() bool Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { + bool updated; + /* check if anything has changed */ - bool onboard_updated = is_onboard_mission_updated(); - bool offboard_updated = is_offboard_mission_updated(); + bool onboard_updated; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } - bool updated = false; + bool offboard_updated; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } /* reset mission items if needed */ - if (onboard_updated || offboard_updated) { + if (onboard_updated || offboard_updated || _first_run) { set_mission_items(pos_sp_triplet); updated = true; _first_run = false; @@ -115,164 +121,9 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) return updated; } -bool -Mission::is_mission_item_reached() -{ - /* TODO: count turns */ -#if 0 - if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item.loiter_radius > 0.01f) { - - return false; - } -#endif - - hrt_abstime now = hrt_absolute_time(); - - if (!_waypoint_position_reached) { - - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - float altitude_amsl = _mission_item.altitude_is_relative - ? _mission_item.altitude + _navigator->get_home_position()->alt - : _mission_item.altitude; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, - &dist_xy, &dist_z); - - if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - - /* require only altitude for takeoff */ - if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) { - _waypoint_position_reached = true; - } - } else { - if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { - _waypoint_position_reached = true; - } - } - } - - if (_waypoint_position_reached && !_waypoint_yaw_reached) { - - /* TODO: removed takeoff, why? */ - if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { - - /* check yaw if defined only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); - - if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ - _waypoint_yaw_reached = true; - } - - } else { - _waypoint_yaw_reached = true; - } - } - - /* check if the current waypoint was reached */ - if (_waypoint_position_reached && _waypoint_yaw_reached) { - - if (_time_first_inside_orbit == 0) { - _time_first_inside_orbit = now; - - // if (_mission_item.time_inside > 0.01f) { - // mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", - // (double)_mission_item.time_inside); - // } - } - - /* check if the MAV was long enough inside the waypoint orbit */ - if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) { - return true; - } - } - return false; -} - void -Mission::reset_mission_item_reached() -{ - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - _time_first_inside_orbit = 0; -} - -void -Mission::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) -{ - sp->valid = true; - sp->lat = item->lat; - sp->lon = item->lon; - sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; - sp->yaw = item->yaw; - sp->loiter_radius = item->loiter_radius; - sp->loiter_direction = item->loiter_direction; - sp->pitch_min = item->pitch_min; - - if (item->nav_cmd == NAV_CMD_TAKEOFF) { - sp->type = SETPOINT_TYPE_TAKEOFF; - - } else if (item->nav_cmd == NAV_CMD_LAND) { - sp->type = SETPOINT_TYPE_LAND; - - } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - sp->type = SETPOINT_TYPE_LOITER; - - } else { - sp->type = SETPOINT_TYPE_POSITION; - } -} - -bool -Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet) +Mission::update_onboard_mission() { - if (_navigator->get_is_in_loiter()) { - /* already loitering, bail out */ - return false; - } - - if (reuse_current_pos_sp && pos_sp_triplet->current.valid) { - /* leave position setpoint as is */ - } else { - /* use current position */ - pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; - pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; - pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; - pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */ - } - pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; - pos_sp_triplet->current.loiter_radius = _param_loiter_radius.get(); - pos_sp_triplet->current.loiter_direction = 1; - - pos_sp_triplet->previous.valid = false; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->next.valid = false; - - _navigator->set_is_in_loiter(true); - return true; -} - - -bool -Mission::is_onboard_mission_updated() -{ - bool updated; - orb_check(_navigator->get_onboard_mission_sub(), &updated); - - if (!updated && !_first_run) { - return false; - } - if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { /* accept the current index set by the onboard mission if it is within bounds */ if (_onboard_mission.current_index >=0 @@ -293,18 +144,11 @@ Mission::is_onboard_mission_updated() _onboard_mission.current_index = 0; _current_onboard_mission_index = 0; } - return true; } -bool -Mission::is_offboard_mission_updated() +void +Mission::update_offboard_mission() { - bool updated; - orb_check(_navigator->get_offboard_mission_sub(), &updated); - - if (!updated && !_first_run) { - return false; - } if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { /* determine current index */ @@ -341,7 +185,6 @@ Mission::is_offboard_mission_updated() _offboard_mission.current_index = 0; _current_offboard_mission_index = 0; } - return true; } @@ -381,7 +224,6 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) _mission_type = MISSION_TYPE_NONE; bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached; - reset_mission_item_reached(); set_loiter_item(use_current_pos_sp, pos_sp_triplet); } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4d0083d85..ad8cb467c 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -33,7 +33,7 @@ /** * @file mission.h * - * Helper class to access missions + * Navigator mode to access missions * * @author Julian Oes */ @@ -50,16 +50,19 @@ #include #include +#include #include #include #include #include +#include "navigator_mode.h" +#include "mission_block.h" #include "mission_feasibility_checker.h" class Navigator; -class Mission : public control::SuperBlock +class Mission : public NavigatorMode, MissionBlock { public: /** @@ -82,42 +85,16 @@ public: */ virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); -protected: - /** - * Check if mission item has been reached - * @return true if successfully reached - */ - bool is_mission_item_reached(); - /** - * Reset all reached flags - */ - void reset_mission_item_reached(); - - /** - * Convert a mission item to a position setpoint - */ - void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp); - - /** - * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position - * @return true if setpoint has changed - */ - bool set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet); - - class Navigator *_navigator; - private: /** * Update onboard mission topic - * @return true if onboard mission has been updated */ - bool is_onboard_mission_updated(); + void update_onboard_mission(); /** * Update offboard mission topic - * @return true if offboard mission has been updated */ - bool is_offboard_mission_updated(); + void update_offboard_mission(); /** * Move on to next mission item or switch to loiter @@ -179,12 +156,6 @@ private: */ void publish_mission_result(); - bool _waypoint_position_reached; - bool _waypoint_yaw_reached; - hrt_abstime _time_first_inside_orbit; - - bool _first_run; - control::BlockParamFloat _param_onboard_enabled; control::BlockParamFloat _param_loiter_radius; @@ -194,8 +165,6 @@ private: int _current_onboard_mission_index; int _current_offboard_mission_index; - struct mission_item_s _mission_item; - orb_advert_t _mission_result_pub; struct mission_result_s _mission_result; @@ -206,7 +175,6 @@ private: } _mission_type; MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ - }; #endif diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp new file mode 100644 index 000000000..92090d995 --- /dev/null +++ b/src/modules/navigator/mission_block.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file mission_block.cpp + * + * Helper class to use mission items + * + * @author Julian Oes + */ + +#include +#include +#include +#include + +#include +#include + +#include + +#include "navigator.h" +#include "mission_block.h" + + +MissionBlock::MissionBlock(Navigator *navigator) : + _waypoint_position_reached(false), + _waypoint_yaw_reached(false), + _time_first_inside_orbit(0), + _mission_item({0}), + _mission_item_valid(false), + _navigator_priv(navigator) +{ +} + +MissionBlock::~MissionBlock() +{ +} + +bool +MissionBlock::is_mission_item_reached() +{ + /* TODO: count turns */ +#if 0 + if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item.loiter_radius > 0.01f) { + + return false; + } +#endif + + hrt_abstime now = hrt_absolute_time(); + + if (!_waypoint_position_reached) { + + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + float altitude_amsl = _mission_item.altitude_is_relative + ? _mission_item.altitude + _navigator_priv->get_home_position()->alt + : _mission_item.altitude; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, + _navigator_priv->get_global_position()->lat, + _navigator_priv->get_global_position()->lon, + _navigator_priv->get_global_position()->alt, + &dist_xy, &dist_z); + + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + + /* require only altitude for takeoff */ + if (_navigator_priv->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } else { + if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } + } + + if (_waypoint_position_reached && !_waypoint_yaw_reached) { + + /* TODO: removed takeoff, why? */ + if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { + + /* check yaw if defined only for rotary wing except takeoff */ + float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw); + + if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ + _waypoint_yaw_reached = true; + } + + } else { + _waypoint_yaw_reached = true; + } + } + + /* check if the current waypoint was reached */ + if (_waypoint_position_reached && _waypoint_yaw_reached) { + + if (_time_first_inside_orbit == 0) { + _time_first_inside_orbit = now; + + // if (_mission_item.time_inside > 0.01f) { + // mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", + // (double)_mission_item.time_inside); + // } + } + + /* check if the MAV was long enough inside the waypoint orbit */ + if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) { + return true; + } + } + return false; +} + +void +MissionBlock::reset_mission_item_reached() +{ + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_first_inside_orbit = 0; +} + +void +MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) +{ + sp->valid = true; + sp->lat = item->lat; + sp->lon = item->lon; + sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude; + sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; + + if (item->nav_cmd == NAV_CMD_TAKEOFF) { + sp->type = SETPOINT_TYPE_TAKEOFF; + + } else if (item->nav_cmd == NAV_CMD_LAND) { + sp->type = SETPOINT_TYPE_LAND; + + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + sp->type = SETPOINT_TYPE_LOITER; + + } else { + sp->type = SETPOINT_TYPE_POSITION; + } +} + +bool +MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet) +{ + if (_navigator_priv->get_is_in_loiter()) { + /* already loitering, bail out */ + return false; + } + + if (reuse_current_pos_sp && pos_sp_triplet->current.valid) { + /* leave position setpoint as is */ + } else { + /* use current position */ + pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon; + pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt; + pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */ + } + pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; + pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius(); + pos_sp_triplet->current.loiter_direction = 1; + + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; + + _navigator_priv->set_is_in_loiter(true); + return true; +} + diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h new file mode 100644 index 000000000..47e6fe81f --- /dev/null +++ b/src/modules/navigator/mission_block.h @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file mission_block.h + * + * Helper class to use mission items + * + * @author Julian Oes + */ + +#ifndef NAVIGATOR_MISSION_BLOCK_H +#define NAVIGATOR_MISSION_BLOCK_H + +#include + +#include +#include +#include + +class Navigator; + +class MissionBlock +{ +public: + /** + * Constructor + * + * @param pointer to parent class + */ + MissionBlock(Navigator *navigator); + + /** + * Destructor + */ + virtual ~MissionBlock(); + + /** + * Check if mission item has been reached + * @return true if successfully reached + */ + bool is_mission_item_reached(); + /** + * Reset all reached flags + */ + void reset_mission_item_reached(); + + /** + * Convert a mission item to a position setpoint + * + * @param the mission item to convert + * @param the position setpoint that needs to be set + */ + void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); + + /** + * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position + * + * @param true if the current position setpoint should be re-used + * @param the position setpoint triplet to set + * @return true if setpoint has changed + */ + bool set_loiter_item(const bool reuse_current_pos_sp, position_setpoint_triplet_s *pos_sp_triplet); + + bool _waypoint_position_reached; + bool _waypoint_yaw_reached; + hrt_abstime _time_first_inside_orbit; + + mission_item_s _mission_item; + bool _mission_item_valid; + +private: + Navigator *_navigator_priv; +}; + +#endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index f4243c5b8..c7cba64cc 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -38,6 +38,8 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ + navigator_mode.cpp \ + mission_block.cpp \ mission.cpp \ mission_params.c \ loiter.cpp \ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 0c551bb41..476e9a36c 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -48,6 +48,7 @@ #include #include +#include "navigator_mode.h" #include "mission.h" #include "loiter.h" #include "rtl.h" @@ -104,6 +105,8 @@ public: Geofence& get_geofence() { return _geofence; } bool get_is_in_loiter() { return _is_in_loiter; } + float get_loiter_radius() { return 50.0f; }; /* TODO: make param*/ + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -139,6 +142,7 @@ private: bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ + NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 44c1075c1..fe1f4893e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -118,6 +118,7 @@ Navigator::Navigator() : _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), + _navigation_mode(nullptr), _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), @@ -321,32 +322,47 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: - _mission.reset(); - _loiter.reset(); - _rtl.reset(); + _navigation_mode = nullptr; _is_in_loiter = false; break; case NAVIGATION_STATE_AUTO_MISSION: - _update_triplet = _mission.update(&_pos_sp_triplet); + _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: - _update_triplet = _loiter.update(&_pos_sp_triplet); + _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTL_RC: case NAVIGATION_STATE_AUTO_RTL_DL: - _update_triplet = _rtl.update(&_pos_sp_triplet); + _navigation_mode = &_rtl; break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: default: - _mission.reset(); - _loiter.reset(); - _rtl.reset(); + _navigation_mode = nullptr; _is_in_loiter = false; break; } + /* TODO: make list of modes and loop through it */ + if (_navigation_mode == &_mission) { + _update_triplet = _mission.update(&_pos_sp_triplet); + } else { + _mission.reset(); + } + + if (_navigation_mode == &_rtl) { + _update_triplet = _rtl.update(&_pos_sp_triplet); + } else { + _rtl.reset(); + } + + if (_navigation_mode == &_loiter) { + _update_triplet = _loiter.update(&_pos_sp_triplet); + } else { + _loiter.reset(); + } + if (_update_triplet ) { publish_position_setpoint_triplet(); _update_triplet = false; diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp new file mode 100644 index 000000000..c96829190 --- /dev/null +++ b/src/modules/navigator/navigator_mode.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mode.cpp + * + * Helper class for different modes in navigator + * + * @author Julian Oes + */ + +#include "navigator_mode.h" + +NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : + SuperBlock(NULL, name), + _navigator(navigator), + _first_run(true) +{ + /* load initial params */ + updateParams(); + /* set initial mission items */ + reset(); +} + +NavigatorMode::~NavigatorMode() +{ +} + +void +NavigatorMode::reset() +{ + _first_run = true; +} + +bool +NavigatorMode::update(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + pos_sp_triplet->current.valid = false; + _first_run = false; + return false; +} diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h new file mode 100644 index 000000000..0844bb94d --- /dev/null +++ b/src/modules/navigator/navigator_mode.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mode.h + * + * Helper class for different modes in navigator + * + * @author Julian Oes + */ + +#ifndef NAVIGATOR_MODE_H +#define NAVIGATOR_MODE_H + +#include + +#include +#include + +#include + +#include + +class Navigator; + +class NavigatorMode : public control::SuperBlock +{ +public: + /** + * Constructor + */ + NavigatorMode(Navigator *navigator, const char *name); + + /** + * Destructor + */ + virtual ~NavigatorMode(); + + /** + * This function is called while the mode is inactive + */ + virtual void reset(); + + /** + * This function is called while the mode is active + * + * @param position setpoint triplet to set + * @return true if position setpoint triplet has been changed + */ + virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + +protected: + Navigator *_navigator; + bool _first_run; +}; + +#endif diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index d4e609584..774aa802d 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -52,13 +52,13 @@ #include "rtl.h" RTL::RTL(Navigator *navigator, const char *name) : - Mission(navigator, name), + NavigatorMode(navigator, name), + MissionBlock(navigator), _mavlink_fd(-1), _rtl_state(RTL_STATE_NONE), _home_position({}), _loiter_radius(50), _acceptance_radius(50), - _param_loiter_rad(this, "LOITER_RAD"), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), _param_land_delay(this, "LAND_DELAY") diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index d84fd1a6f..9b32fbb0c 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -31,8 +31,9 @@ * ****************************************************************************/ /** - * @file navigator_mission.h - * Helper class to access missions + * @file navigator_rtl.h + * Helper class for RTL + * * @author Julian Oes * @author Anton Babushkin */ @@ -48,11 +49,12 @@ #include #include -#include "mission.h" +#include "navigator_mode.h" +#include "mission_block.h" class Navigator; -class RTL : public Mission +class RTL : public NavigatorMode, MissionBlock { public: /** @@ -63,11 +65,18 @@ public: /** * Destructor */ - virtual ~RTL(); + ~RTL(); + + /** + * This function is called while the mode is inactive + */ + void reset(); - virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + /** + * This function is called while the mode is active + */ + bool update(struct position_setpoint_triplet_s *pos_sp_triplet); - virtual void reset(); private: void set_home_position(const home_position_s *home_position); @@ -92,7 +101,6 @@ private: float _loiter_radius; float _acceptance_radius; - control::BlockParamFloat _param_loiter_rad; control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; -- cgit v1.2.3 From 85d7fdc741a39184d251e2d35d914a6506d6ecd1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 18:13:45 +0200 Subject: navigator: param enhancements --- src/modules/navigator/mission.cpp | 3 +- src/modules/navigator/mission.h | 1 - src/modules/navigator/mission_params.c | 10 ------ src/modules/navigator/module.mk | 1 + src/modules/navigator/navigator.h | 15 +++++++-- src/modules/navigator/navigator_main.cpp | 26 ++++++++++++--- src/modules/navigator/navigator_params.c | 55 ++++++++++++++++++++++++++++++++ 7 files changed, 93 insertions(+), 18 deletions(-) create mode 100644 src/modules/navigator/navigator_params.c diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 167f24ca6..93007d939 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -60,7 +60,6 @@ Mission::Mission(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), MissionBlock(navigator), _param_onboard_enabled(this, "ONBOARD_EN"), - _param_loiter_radius(this, "LOITER_RAD"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -241,6 +240,8 @@ Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos bool Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { + /* make sure param is up to date */ + updateParams(); if (_param_onboard_enabled.get() > 0 && _current_onboard_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index ad8cb467c..cb3242c87 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -157,7 +157,6 @@ private: void publish_mission_result(); control::BlockParamFloat _param_onboard_enabled; - control::BlockParamFloat _param_loiter_radius; struct mission_s _onboard_mission; struct mission_s _offboard_mission; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index f5067a70b..8692328db 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -58,16 +58,6 @@ */ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); -/** - * Loiter radius after/during mission (FW only) - * - * Default value of loiter radius (fixedwing only). - * - * @unit meters - * @min 0.0 - * @group Mission - */ -PARAM_DEFINE_FLOAT(MIS_LOITER_RAD, 50.0f); /** * Enable onboard mission diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index c7cba64cc..a1e109030 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ + navigator_params.c \ navigator_mode.cpp \ mission_block.cpp \ mission.cpp \ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 476e9a36c..b22da7117 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -42,11 +42,15 @@ #include +#include +#include + #include #include #include #include #include +#include #include "navigator_mode.h" #include "mission.h" @@ -54,7 +58,7 @@ #include "rtl.h" #include "geofence.h" -class Navigator +class Navigator : public control::SuperBlock { public: /** @@ -105,7 +109,7 @@ public: Geofence& get_geofence() { return _geofence; } bool get_is_in_loiter() { return _is_in_loiter; } - float get_loiter_radius() { return 50.0f; }; /* TODO: make param*/ + float get_loiter_radius() { return _param_loiter_radius.get(); }; private: @@ -121,6 +125,7 @@ private: int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ + int _param_update_sub; /**< param update subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ @@ -150,6 +155,7 @@ private: bool _is_in_loiter; /**< flags if current position SP can be used to loiter */ bool _update_triplet; /**< flags if position SP triplet needs to be published */ + control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ /** * Retrieve global position */ @@ -175,6 +181,11 @@ private: */ void vehicle_control_mode_update(); + /** + * Update parameters + */ + void params_update(); + /** * Shim for calling task_main from task_create. */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fe1f4893e..b1658a6b8 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -92,8 +92,7 @@ Navigator *g_navigator; } Navigator::Navigator() : - - /* state machine transition table */ + SuperBlock(NULL, "NAV"), _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), @@ -122,8 +121,10 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _update_triplet(false) + _update_triplet(false), + _param_loiter_radius(this, "LOITER_RAD") { + updateParams(); } Navigator::~Navigator() @@ -188,6 +189,13 @@ Navigator::vehicle_control_mode_update() } } +void +Navigator::params_update() +{ + parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶m_update); +} + void Navigator::task_main_trampoline(int argc, char *argv[]) { @@ -226,6 +234,7 @@ Navigator::task_main() _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + _param_update_sub - orb_subscribe(ORB_ID(parameter_update)); /* copy all topics first time */ vehicle_status_update(); @@ -233,6 +242,7 @@ Navigator::task_main() global_position_update(); home_position_update(); navigation_capabilities_update(); + params_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -241,7 +251,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[5]; + struct pollfd fds[6]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -254,6 +264,8 @@ Navigator::task_main() fds[3].events = POLLIN; fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; + fds[5].fd = _param_update_sub; + fds[5].events = POLLIN; while (!_task_should_exit) { @@ -278,6 +290,12 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* parameters updated */ + if (fds[5].revents & POLLIN) { + params_update(); + updateParams(); + } + /* vehicle control mode updated */ if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c new file mode 100644 index 000000000..9235f3046 --- /dev/null +++ b/src/modules/navigator/navigator_params.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file navigator_params.c + * + * Parameters for navigator in general + * + * @author Julian Oes + */ + +#include + +#include + +/** + * Loiter radius (FW only) + * + * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). + * + * @unit meters + * @min 0.0 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); -- cgit v1.2.3 From b97b74491bfa7bddf8b7a80525f031e4c83fe5d4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 18:27:59 +0200 Subject: navigator: typo --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b1658a6b8..241d1d8a4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -234,7 +234,7 @@ Navigator::task_main() _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); - _param_update_sub - orb_subscribe(ORB_ID(parameter_update)); + _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); /* copy all topics first time */ vehicle_status_update(); -- cgit v1.2.3 From 1bec9dfe2bab4200a2e2859d1efee0a1bb66e6d1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 20:02:38 +0200 Subject: navigator: RTL working again --- src/modules/navigator/mission.cpp | 2 +- src/modules/navigator/navigator.h | 4 +- src/modules/navigator/rtl.cpp | 222 ++++++++++++++++++++----------------- src/modules/navigator/rtl.h | 25 +++-- src/modules/navigator/rtl_params.c | 12 ++ 5 files changed, 148 insertions(+), 117 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 93007d939..cd694be9a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -88,7 +88,7 @@ Mission::reset() bool Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { - bool updated; + bool updated = false; /* check if anything has changed */ bool onboard_updated; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b22da7117..296294a91 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -108,8 +108,8 @@ public: int get_offboard_mission_sub() { return _offboard_mission_sub; } Geofence& get_geofence() { return _geofence; } bool get_is_in_loiter() { return _is_in_loiter; } - - float get_loiter_radius() { return _param_loiter_radius.get(); }; + float get_loiter_radius() { return _param_loiter_radius.get(); } + int get_mavlink_fd() { return _mavlink_fd; } private: diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 774aa802d..c4c5f2aab 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -49,19 +49,17 @@ #include #include +#include "navigator.h" #include "rtl.h" RTL::RTL(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), MissionBlock(navigator), - _mavlink_fd(-1), _rtl_state(RTL_STATE_NONE), - _home_position({}), - _loiter_radius(50), - _acceptance_radius(50), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY") + _param_land_delay(this, "LAND_DELAY"), + _param_acceptance_radius(this, "ACCEPT_RAD") { /* load initial params */ updateParams(); @@ -73,40 +71,52 @@ RTL::~RTL() { } +void +RTL::reset() +{ + _first_run = true; + _rtl_state = RTL_STATE_NONE; +} + bool RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet) { bool updated = false; - return updated; -} + if (_first_run) { + set_rtl_item(pos_sp_triplet); + updated = true; + _first_run = false; + } -void -RTL::reset() -{ + if ((_rtl_state == RTL_STATE_CLIMB + || _rtl_state == RTL_STATE_RETURN + || _rtl_state == RTL_STATE_DESCEND) + && is_mission_item_reached()) { + advance_rtl(); + set_rtl_item(pos_sp_triplet); + updated = true; + } + return updated; } void -RTL::set_home_position(const home_position_s *new_home_position) -{ - memcpy(&_home_position, new_home_position, sizeof(home_position_s)); -} - -bool -RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item) +RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) { - /* Open mavlink fd */ - if (_mavlink_fd < 0) { - /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - } + /* make sure we have the latest params */ + updateParams(); - /* decide if we need climb */ + /* decide where to enter the RTL procedure when we switch into it */ if (_rtl_state == RTL_STATE_NONE) { - if (global_position->alt < _home_position.alt + _param_return_alt.get()) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_FINISHED; + /* if lower than return altitude, climb up first */ + } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + + _param_return_alt.get()) { _rtl_state = RTL_STATE_CLIMB; - + /* otherwise go straight to return */ } else { _rtl_state = RTL_STATE_RETURN; } @@ -114,129 +124,137 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss /* if switching directly to return state, set altitude setpoint to current altitude */ if (_rtl_state == RTL_STATE_RETURN) { - new_mission_item->altitude_is_relative = false; - new_mission_item->altitude = global_position->alt; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; } switch (_rtl_state) { case RTL_STATE_CLIMB: { - float climb_alt = _home_position.alt + _param_return_alt.get(); + float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); /* TODO understand and fix this */ // if (_vstatus.condition_landed) { // climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); // } - new_mission_item->lat = global_position->lat; - new_mission_item->lon = global_position->lon; - new_mission_item->altitude_is_relative = false; - new_mission_item->altitude = climb_alt; - new_mission_item->yaw = NAN; - new_mission_item->loiter_radius = _loiter_radius; - new_mission_item->loiter_direction = 1; - new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; - new_mission_item->acceptance_radius = _acceptance_radius; - new_mission_item->time_inside = 0.0f; - new_mission_item->pitch_min = 0.0f; - new_mission_item->autocontinue = true; - new_mission_item->origin = ORIGIN_ONBOARD; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %d meters above home", - (int)(climb_alt - _home_position.alt)); + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_is_in_loiter(false); + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", + (int)(climb_alt - _navigator->get_home_position()->alt)); break; } case RTL_STATE_RETURN: { - new_mission_item->lat = _home_position.lat; - new_mission_item->lon = _home_position.lon; + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; /* TODO: add this again */ // don't change altitude // if (_pos_sp_triplet.previous.valid) { // /* if previous setpoint is valid then use it to calculate heading to home */ - // new_mission_item->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, new_mission_item->lat, new_mission_item->lon); + // _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); // } else { // /* else use current position */ - // new_mission_item->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, new_mission_item->lat, new_mission_item->lon); + // _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); // } - new_mission_item->loiter_radius = _loiter_radius; - new_mission_item->loiter_direction = 1; - new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; - new_mission_item->acceptance_radius = _acceptance_radius; - new_mission_item->time_inside = 0.0f; - new_mission_item->pitch_min = 0.0f; - new_mission_item->autocontinue = true; - new_mission_item->origin = ORIGIN_ONBOARD; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %d meters above home", - (int)(new_mission_item->altitude - _home_position.alt)); + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_is_in_loiter(false); + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } - case RTL_STATE_DESCEND: { - new_mission_item->lat = _home_position.lat; - new_mission_item->lon = _home_position.lon; - new_mission_item->altitude_is_relative = false; - new_mission_item->altitude = _home_position.alt + _param_descend_alt.get(); - new_mission_item->yaw = NAN; - new_mission_item->loiter_radius = _loiter_radius; - new_mission_item->loiter_direction = 1; - new_mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - new_mission_item->acceptance_radius = _acceptance_radius; - new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); - new_mission_item->pitch_min = 0.0f; - new_mission_item->autocontinue = _param_land_delay.get() > -0.001f; - new_mission_item->origin = ORIGIN_ONBOARD; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %d meters above home", - (int)(new_mission_item->altitude - _home_position.alt)); + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = _param_land_delay.get() > -0.001f; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_is_in_loiter(true); + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } case RTL_STATE_LAND: { - new_mission_item->lat = _home_position.lat; - new_mission_item->lon = _home_position.lon; - new_mission_item->altitude_is_relative = false; - new_mission_item->altitude = _home_position.alt; - new_mission_item->yaw = NAN; - new_mission_item->loiter_radius = _loiter_radius; - new_mission_item->loiter_direction = 1; - new_mission_item->nav_cmd = NAV_CMD_LAND; - new_mission_item->acceptance_radius = _acceptance_radius; - new_mission_item->time_inside = 0.0f; - new_mission_item->pitch_min = 0.0f; - new_mission_item->autocontinue = true; - new_mission_item->origin = ORIGIN_ONBOARD; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: land at home"); + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_is_in_loiter(false); + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); break; } case RTL_STATE_FINISHED: { /* nothing to do, report fail */ - return false; } default: - return false; + break; } - return true; -} - -bool -RTL::get_next_rtl_item(mission_item_s *new_mission_item) -{ - /* TODO implement */ - return false; + if (_rtl_state == RTL_STATE_FINISHED) { + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + } else { + /* if not finished, convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + reset_mission_item_reached(); + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; + } } void -RTL::move_to_next() +RTL::advance_rtl() { switch (_rtl_state) { case RTL_STATE_CLIMB: diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9b32fbb0c..9836f5064 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -74,19 +74,23 @@ public: /** * This function is called while the mode is active + * + * @param position setpoint triplet that needs to be set + * @return true if updated */ - bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + bool update(position_setpoint_triplet_s *pos_sp_triplet); private: - void set_home_position(const home_position_s *home_position); - - bool get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item); - bool get_next_rtl_item(mission_item_s *mission_item); - - void move_to_next(); + /** + * Set the RTL item + */ + void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet); - int _mavlink_fd; + /** + * Move to next RTL item + */ + void advance_rtl(); enum RTLState { RTL_STATE_NONE = 0, @@ -97,13 +101,10 @@ private: RTL_STATE_FINISHED, } _rtl_state; - home_position_s _home_position; - float _loiter_radius; - float _acceptance_radius; - control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; + control::BlockParamFloat _param_acceptance_radius; }; #endif diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index bfe6ce7e1..63724f461 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -96,3 +96,15 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); * @group RTL */ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); + +/** + * RTL acceptance radius + * + * Acceptance radius for waypoints set for RTL + * + * @unit meters + * @min 1 + * @max + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f); -- cgit v1.2.3 From 94c4fc56aa229bcb909e5437804e7475adfbcdba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 23:08:11 +0200 Subject: navigator: audio messages about what is happening, RTL during mission not triggered but after mission --- src/modules/commander/commander.cpp | 17 ++++++++++- src/modules/navigator/mission.cpp | 60 +++++++++++++++++++++++++++++-------- src/modules/navigator/mission.h | 5 ++++ src/modules/navigator/rtl.cpp | 6 +--- 4 files changed, 70 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d7a95b0d6..bb75b2af0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1296,7 +1296,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ @@ -1306,10 +1305,26 @@ int commander_thread_main(int argc, char *argv[]) status.failsafe_state = FAILSAFE_STATE_LAND; } failsafe_state_changed = true; + } else { + mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission"); } } } + /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ + if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && + mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RTL_RC) { + /* if we have a global position, we can switch to RTL, if not, we can try to land */ + if (status.condition_global_position_valid) { + status.failsafe_state = FAILSAFE_STATE_RTL_RC; + mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); + } else { + /* this probably doesn't make sense since we are in mission and have global position */ + status.failsafe_state = FAILSAFE_STATE_LAND; + } + failsafe_state_changed = true; + } + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cd694be9a..db606e6fa 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -46,6 +46,7 @@ #include #include +#include #include #include @@ -210,20 +211,39 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) { set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); + /* try setting onboard mission item */ if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { - /* try setting onboard mission item */ + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_ONBOARD) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: onboard mission running"); + } _mission_type = MISSION_TYPE_ONBOARD; _navigator->set_is_in_loiter(false); + /* try setting offboard mission item */ } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { - /* try setting offboard mission item */ + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_OFFBOARD) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: offboard mission running"); + } _mission_type = MISSION_TYPE_OFFBOARD; _navigator->set_is_in_loiter(false); } else { + if (_mission_type != MISSION_TYPE_NONE) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: mission finished"); + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: no mission available"); + } _mission_type = MISSION_TYPE_NONE; bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached; set_loiter_item(use_current_pos_sp, pos_sp_triplet); + reset_mission_item_reached(); + report_mission_finished(); } } @@ -242,8 +262,9 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current { /* make sure param is up to date */ updateParams(); - if (_param_onboard_enabled.get() > 0 - && _current_onboard_mission_index < (int)_onboard_mission.count) { + if (_param_onboard_enabled.get() > 0 && + _current_onboard_mission_index >= 0&& + _current_onboard_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, &new_mission_item)) { @@ -264,7 +285,8 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current bool Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { - if (_current_offboard_mission_index < (int)_offboard_mission.count) { + if (_current_offboard_mission_index >= 0 && + _current_offboard_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -282,8 +304,6 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren report_current_offboard_mission_item(); memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); return true; - } else { - warnx("ERROR: WP read fail"); } } return false; @@ -295,7 +315,8 @@ Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) int next_temp_mission_index = _onboard_mission.current_index + 1; /* try if there is a next onboard mission */ - if (next_temp_mission_index < (int)_onboard_mission.count) { + if (_onboard_mission.current_index >= 0 && + next_temp_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { /* convert next mission item to position setpoint */ @@ -315,7 +336,9 @@ Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) { /* try if there is a next offboard mission */ int next_temp_mission_index = _offboard_mission.current_index + 1; - if (next_temp_mission_index < (int)_offboard_mission.count) { + warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); + if (_offboard_mission.current_index >= 0 && + next_temp_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -346,14 +369,17 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* read mission item from datamanager */ if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR waypoint could not be read"); return false; } /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - /* TODO: do this check more gracefully since it is not a serious error */ if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: DO JUMP repetitions completed"); return false; } @@ -366,9 +392,10 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the dataman */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP waypoint could not be written"); return false; } - /* TODO: report about DO JUMP count */ } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ @@ -381,7 +408,8 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } /* we have given up, we don't want to cycle forever */ - warnx("ERROR: cycling through mission items without success"); + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP is cycling, giving up"); return false; } @@ -402,6 +430,13 @@ Mission::report_current_offboard_mission_item() publish_mission_result(); } +void +Mission::report_mission_finished() +{ + _mission_result.mission_finished = true; + publish_mission_result(); +} + void Mission::publish_mission_result() { @@ -416,4 +451,5 @@ Mission::publish_mission_result() } /* reset reached bool */ _mission_result.mission_reached = false; + _mission_result.mission_finished = false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index cb3242c87..a3dd09ecd 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -151,6 +151,11 @@ private: */ void report_current_offboard_mission_item(); + /** + * Report that the mission is finished if one exists or that none exists + */ + void report_mission_finished(); + /** * Publish the mission result so commander and mavlink know what is going on */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index c4c5f2aab..dde35d5b6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -112,6 +112,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) /* for safety reasons don't go into RTL if landed */ if (_navigator->get_vstatus()->condition_landed) { _rtl_state = RTL_STATE_FINISHED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); /* if lower than return altitude, climb up first */ } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) { @@ -133,11 +134,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); - /* TODO understand and fix this */ - // if (_vstatus.condition_landed) { - // climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - // } - _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; _mission_item.altitude_is_relative = false; -- cgit v1.2.3 From 5bf68dad940b2c2641796badd928faea9fb963a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 12:52:06 +0200 Subject: Rename / move 23 state filter --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2625 -------------------- src/modules/ekf_att_pos_estimator/estimator.h | 356 --- .../ekf_att_pos_estimator/estimator_23states.cpp | 2488 +++++++++++++++++++ .../ekf_att_pos_estimator/estimator_23states.h | 282 +++ 4 files changed, 2770 insertions(+), 2981 deletions(-) delete mode 100644 src/modules/ekf_att_pos_estimator/estimator.cpp delete mode 100644 src/modules/ekf_att_pos_estimator/estimator.h create mode 100644 src/modules/ekf_att_pos_estimator/estimator_23states.cpp create mode 100644 src/modules/ekf_att_pos_estimator/estimator_23states.h diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp deleted file mode 100644 index 89e137adc..000000000 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ /dev/null @@ -1,2625 +0,0 @@ -#include "estimator.h" -#include -#include - -// Define EKF_DEBUG here to enable the debug print calls -// if the macro is not set, these will be completely -// optimized out by the compiler. -//#define EKF_DEBUG - -#ifdef EKF_DEBUG -#include - -static void -ekf_debug_print(const char *fmt, va_list args) -{ - fprintf(stderr, "%s: ", "[ekf]"); - vfprintf(stderr, fmt, args); - - fprintf(stderr, "\n"); -} - -static void -ekf_debug(const char *fmt, ...) -{ - va_list args; - - va_start(args, fmt); - ekf_debug_print(fmt, args); -} - -#else - -static void ekf_debug(const char *fmt, ...) { while(0){} } -#endif - -float Vector3f::length(void) const -{ - return sqrt(x*x + y*y + z*z); -} - -void Vector3f::zero(void) -{ - x = 0.0f; - y = 0.0f; - z = 0.0f; -} - -Mat3f::Mat3f() { - identity(); -} - -void Mat3f::identity() { - x.x = 1.0f; - x.y = 0.0f; - x.z = 0.0f; - - y.x = 0.0f; - y.y = 1.0f; - y.z = 0.0f; - - z.x = 0.0f; - z.y = 0.0f; - z.z = 1.0f; -} - -Mat3f Mat3f::transpose(void) const -{ - Mat3f ret = *this; - swap_var(ret.x.y, ret.y.x); - swap_var(ret.x.z, ret.z.x); - swap_var(ret.y.z, ret.z.y); - return ret; -} - -// overload + operator to provide a vector addition -Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x + vecIn2.x; - vecOut.y = vecIn1.y + vecIn2.y; - vecOut.z = vecIn1.z + vecIn2.z; - return vecOut; -} - -// overload - operator to provide a vector subtraction -Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x - vecIn2.x; - vecOut.y = vecIn1.y - vecIn2.y; - vecOut.z = vecIn1.z - vecIn2.z; - return vecOut; -} - -// overload * operator to provide a matrix vector product -Vector3f operator*( Mat3f matIn, Vector3f vecIn) -{ - Vector3f vecOut; - vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; - vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; - vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; - return vecOut; -} - -// overload % operator to provide a vector cross product -Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) -{ - Vector3f vecOut; - vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y; - vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z; - vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x; - return vecOut; -} - -// overload * operator to provide a vector scaler product -Vector3f operator*(Vector3f vecIn1, float sclIn1) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x * sclIn1; - vecOut.y = vecIn1.y * sclIn1; - vecOut.z = vecIn1.z * sclIn1; - return vecOut; -} - -// overload * operator to provide a vector scaler product -Vector3f operator*(float sclIn1, Vector3f vecIn1) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x * sclIn1; - vecOut.y = vecIn1.y * sclIn1; - vecOut.z = vecIn1.z * sclIn1; - return vecOut; -} - -void swap_var(float &d1, float &d2) -{ - float tmp = d1; - d1 = d2; - d2 = tmp; -} - -AttPosEKF::AttPosEKF() - - /* NOTE: DO NOT initialize class members here. Use ZeroVariables() - * instead to allow clean in-air re-initialization. - */ -{ - memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - ZeroVariables(); - InitialiseParameters(); -} - -AttPosEKF::~AttPosEKF() -{ -} - -void AttPosEKF::UpdateStrapdownEquationsNED() -{ - Vector3f delVelNav; - float q00; - float q11; - float q22; - float q33; - float q01; - float q02; - float q03; - float q12; - float q13; - float q23; - float rotationMag; - float qUpdated[4]; - float quatMag; - float deltaQuat[4]; - const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; - -// Remove sensor bias errors - correctedDelAng.x = dAngIMU.x - states[10]; - correctedDelAng.y = dAngIMU.y - states[11]; - correctedDelAng.z = dAngIMU.z - states[12]; - dVelIMU.x = dVelIMU.x; - dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z - states[13]; - -// Save current measurements - Vector3f prevDelAng = correctedDelAng; - -// Apply corrections for earths rotation rate and coning errors -// * and + operators have been overloaded - correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); - -// Convert the rotation vector to its equivalent quaternion - rotationMag = correctedDelAng.length(); - if (rotationMag < 1e-12f) - { - deltaQuat[0] = 1.0; - deltaQuat[1] = 0.0; - deltaQuat[2] = 0.0; - deltaQuat[3] = 0.0; - } - else - { - deltaQuat[0] = cosf(0.5f*rotationMag); - float rotScaler = (sinf(0.5f*rotationMag))/rotationMag; - deltaQuat[1] = correctedDelAng.x*rotScaler; - deltaQuat[2] = correctedDelAng.y*rotScaler; - deltaQuat[3] = correctedDelAng.z*rotScaler; - } - -// Update the quaternions by rotating from the previous attitude through -// the delta angle rotation quaternion - qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; - qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; - qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; - qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; - -// Normalise the quaternions and update the quaternion states - quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); - if (quatMag > 1e-16f) - { - float quatMagInv = 1.0f/quatMag; - states[0] = quatMagInv*qUpdated[0]; - states[1] = quatMagInv*qUpdated[1]; - states[2] = quatMagInv*qUpdated[2]; - states[3] = quatMagInv*qUpdated[3]; - } - -// Calculate the body to nav cosine matrix - q00 = sq(states[0]); - q11 = sq(states[1]); - q22 = sq(states[2]); - q33 = sq(states[3]); - q01 = states[0]*states[1]; - q02 = states[0]*states[2]; - q03 = states[0]*states[3]; - q12 = states[1]*states[2]; - q13 = states[1]*states[3]; - q23 = states[2]*states[3]; - - Tbn.x.x = q00 + q11 - q22 - q33; - Tbn.y.y = q00 - q11 + q22 - q33; - Tbn.z.z = q00 - q11 - q22 + q33; - Tbn.x.y = 2*(q12 - q03); - Tbn.x.z = 2*(q13 + q02); - Tbn.y.x = 2*(q12 + q03); - Tbn.y.z = 2*(q23 - q01); - Tbn.z.x = 2*(q13 - q02); - Tbn.z.y = 2*(q23 + q01); - - Tnb = Tbn.transpose(); - -// transform body delta velocities to delta velocities in the nav frame -// * and + operators have been overloaded - //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; - delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; - delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; - delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; - -// calculate the magnitude of the nav acceleration (required for GPS -// variance estimation) - accNavMag = delVelNav.length()/dtIMU; - -// If calculating position save previous velocity - float lastVelocity[3]; - lastVelocity[0] = states[4]; - lastVelocity[1] = states[5]; - lastVelocity[2] = states[6]; - -// Sum delta velocities to get velocity - states[4] = states[4] + delVelNav.x; - states[5] = states[5] + delVelNav.y; - states[6] = states[6] + delVelNav.z; - -// If calculating postions, do a trapezoidal integration for position - states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; - states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; - states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; - - // Constrain states (to protect against filter divergence) - ConstrainStates(); -} - -void AttPosEKF::CovariancePrediction(float dt) -{ - // scalars - float daxCov; - float dayCov; - float dazCov; - float dvxCov; - float dvyCov; - float dvzCov; - float dvx; - float dvy; - float dvz; - float dax; - float day; - float daz; - float q0; - float q1; - float q2; - float q3; - float dax_b; - float day_b; - float daz_b; - float dvz_b; - - // arrays - float processNoise[n_states]; - float SF[15]; - float SG[8]; - float SQ[11]; - float SPP[8] = {0}; - float nextP[n_states][n_states]; - - // calculate covariance prediction process noise - for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; - for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; - // scale gyro bias noise when on ground to allow for faster bias estimation - for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; - processNoise[13] = dVelBiasSigma; - for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; - for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; - for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; - processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; - - // square all sigmas - for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); - - // set variables used to calculate covariance growth - dvx = summedDelVel.x; - dvy = summedDelVel.y; - dvz = summedDelVel.z; - dax = summedDelAng.x; - day = summedDelAng.y; - daz = summedDelAng.z; - q0 = states[0]; - q1 = states[1]; - q2 = states[2]; - q3 = states[3]; - dax_b = states[10]; - day_b = states[11]; - daz_b = states[12]; - dvz_b = states[13]; - gyroProcessNoise = ConstrainFloat(gyroProcessNoise, 1e-3f, 5e-2f); - daxCov = sq(dt*gyroProcessNoise); - dayCov = sq(dt*gyroProcessNoise); - dazCov = sq(dt*gyroProcessNoise); - if (onGround) dazCov = dazCov * sq(yawVarScale); - accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f); - dvxCov = sq(dt*accelProcessNoise); - dvyCov = sq(dt*accelProcessNoise); - dvzCov = sq(dt*accelProcessNoise); - - // Predicted covariance calculation - SF[0] = dvz - dvz_b; - SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2; - SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; - SF[3] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; - SF[4] = day/2 - day_b/2; - SF[5] = daz/2 - daz_b/2; - SF[6] = dax/2 - dax_b/2; - SF[7] = dax_b/2 - dax/2; - SF[8] = daz_b/2 - daz/2; - SF[9] = day_b/2 - day/2; - SF[10] = 2*q0*SF[0]; - SF[11] = q1/2; - SF[12] = q2/2; - SF[13] = q3/2; - SF[14] = 2*dvy*q1; - - SG[0] = q0/2; - SG[1] = sq(q3); - SG[2] = sq(q2); - SG[3] = sq(q1); - SG[4] = sq(q0); - SG[5] = 2*q2*q3; - SG[6] = 2*q1*q3; - SG[7] = 2*q1*q2; - - SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); - SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); - SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); - SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4; - SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4; - SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4; - SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4; - SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2; - SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; - SQ[9] = sq(SG[0]); - SQ[10] = sq(q1); - - SPP[0] = SF[10] + SF[14] - 2*dvx*q2; - SPP[1] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; - SPP[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; - SPP[3] = 2*q0*q1 - 2*q2*q3; - SPP[4] = 2*q0*q2 + 2*q1*q3; - SPP[5] = sq(q0) - sq(q1) - sq(q2) + sq(q3); - SPP[6] = SF[13]; - SPP[7] = SF[12]; - - nextP[0][0] = P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6] + (daxCov*SQ[10])/4 + SF[7]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[9]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[8]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) + SPP[7]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[6]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; - nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6] + SF[6]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[5]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[9]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[6]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) - SPP[7]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - (q0*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]))/2; - nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6] + SF[4]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[8]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[6]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - SPP[6]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]))/2; - nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6] + SF[5]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[4]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[7]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SF[11]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[7]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]))/2; - nextP[0][4] = P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6] + SF[3]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[0]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SPP[2]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[4]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); - nextP[0][5] = P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6] + SF[2]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[3]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[0]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[3]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); - nextP[0][6] = P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6] + SF[2]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[1]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[0]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) - SPP[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); - nextP[0][7] = P[0][7] + P[1][7]*SF[7] + P[2][7]*SF[9] + P[3][7]*SF[8] + P[10][7]*SF[11] + P[11][7]*SPP[7] + P[12][7]*SPP[6] + dt*(P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6]); - nextP[0][8] = P[0][8] + P[1][8]*SF[7] + P[2][8]*SF[9] + P[3][8]*SF[8] + P[10][8]*SF[11] + P[11][8]*SPP[7] + P[12][8]*SPP[6] + dt*(P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6]); - nextP[0][9] = P[0][9] + P[1][9]*SF[7] + P[2][9]*SF[9] + P[3][9]*SF[8] + P[10][9]*SF[11] + P[11][9]*SPP[7] + P[12][9]*SPP[6] + dt*(P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6]); - nextP[0][10] = P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]; - nextP[0][11] = P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]; - nextP[0][12] = P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]; - nextP[0][13] = P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]; - nextP[0][14] = P[0][14] + P[1][14]*SF[7] + P[2][14]*SF[9] + P[3][14]*SF[8] + P[10][14]*SF[11] + P[11][14]*SPP[7] + P[12][14]*SPP[6]; - nextP[0][15] = P[0][15] + P[1][15]*SF[7] + P[2][15]*SF[9] + P[3][15]*SF[8] + P[10][15]*SF[11] + P[11][15]*SPP[7] + P[12][15]*SPP[6]; - nextP[0][16] = P[0][16] + P[1][16]*SF[7] + P[2][16]*SF[9] + P[3][16]*SF[8] + P[10][16]*SF[11] + P[11][16]*SPP[7] + P[12][16]*SPP[6]; - nextP[0][17] = P[0][17] + P[1][17]*SF[7] + P[2][17]*SF[9] + P[3][17]*SF[8] + P[10][17]*SF[11] + P[11][17]*SPP[7] + P[12][17]*SPP[6]; - nextP[0][18] = P[0][18] + P[1][18]*SF[7] + P[2][18]*SF[9] + P[3][18]*SF[8] + P[10][18]*SF[11] + P[11][18]*SPP[7] + P[12][18]*SPP[6]; - nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6]; - nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6]; - nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6]; - nextP[0][22] = P[0][22] + P[1][22]*SF[7] + P[2][22]*SF[9] + P[3][22]*SF[8] + P[10][22]*SF[11] + P[11][22]*SPP[7] + P[12][22]*SPP[6]; - nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2); - nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2; - nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2; - nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[4]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SF[11]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[7]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2))/2; - nextP[1][4] = P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[4]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); - nextP[1][5] = P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); - nextP[1][6] = P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2 + SF[2]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[1]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); - nextP[1][7] = P[1][7] + P[0][7]*SF[6] + P[2][7]*SF[5] + P[3][7]*SF[9] + P[11][7]*SPP[6] - P[12][7]*SPP[7] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2); - nextP[1][8] = P[1][8] + P[0][8]*SF[6] + P[2][8]*SF[5] + P[3][8]*SF[9] + P[11][8]*SPP[6] - P[12][8]*SPP[7] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2); - nextP[1][9] = P[1][9] + P[0][9]*SF[6] + P[2][9]*SF[5] + P[3][9]*SF[9] + P[11][9]*SPP[6] - P[12][9]*SPP[7] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2); - nextP[1][10] = P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2; - nextP[1][11] = P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2; - nextP[1][12] = P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2; - nextP[1][13] = P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2; - nextP[1][14] = P[1][14] + P[0][14]*SF[6] + P[2][14]*SF[5] + P[3][14]*SF[9] + P[11][14]*SPP[6] - P[12][14]*SPP[7] - (P[10][14]*q0)/2; - nextP[1][15] = P[1][15] + P[0][15]*SF[6] + P[2][15]*SF[5] + P[3][15]*SF[9] + P[11][15]*SPP[6] - P[12][15]*SPP[7] - (P[10][15]*q0)/2; - nextP[1][16] = P[1][16] + P[0][16]*SF[6] + P[2][16]*SF[5] + P[3][16]*SF[9] + P[11][16]*SPP[6] - P[12][16]*SPP[7] - (P[10][16]*q0)/2; - nextP[1][17] = P[1][17] + P[0][17]*SF[6] + P[2][17]*SF[5] + P[3][17]*SF[9] + P[11][17]*SPP[6] - P[12][17]*SPP[7] - (P[10][17]*q0)/2; - nextP[1][18] = P[1][18] + P[0][18]*SF[6] + P[2][18]*SF[5] + P[3][18]*SF[9] + P[11][18]*SPP[6] - P[12][18]*SPP[7] - (P[10][18]*q0)/2; - nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2; - nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2; - nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2; - nextP[1][22] = P[1][22] + P[0][22]*SF[6] + P[2][22]*SF[5] + P[3][22]*SF[9] + P[11][22]*SPP[6] - P[12][22]*SPP[7] - (P[10][22]*q0)/2; - nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2); - nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2; - nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2; - nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[7]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SF[11]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[7]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2))/2; - nextP[2][4] = P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[4]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); - nextP[2][5] = P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); - nextP[2][6] = P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2 + SF[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[1]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); - nextP[2][7] = P[2][7] + P[0][7]*SF[4] + P[1][7]*SF[8] + P[3][7]*SF[6] + P[12][7]*SF[11] - P[10][7]*SPP[6] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2); - nextP[2][8] = P[2][8] + P[0][8]*SF[4] + P[1][8]*SF[8] + P[3][8]*SF[6] + P[12][8]*SF[11] - P[10][8]*SPP[6] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2); - nextP[2][9] = P[2][9] + P[0][9]*SF[4] + P[1][9]*SF[8] + P[3][9]*SF[6] + P[12][9]*SF[11] - P[10][9]*SPP[6] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2); - nextP[2][10] = P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2; - nextP[2][11] = P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2; - nextP[2][12] = P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2; - nextP[2][13] = P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2; - nextP[2][14] = P[2][14] + P[0][14]*SF[4] + P[1][14]*SF[8] + P[3][14]*SF[6] + P[12][14]*SF[11] - P[10][14]*SPP[6] - (P[11][14]*q0)/2; - nextP[2][15] = P[2][15] + P[0][15]*SF[4] + P[1][15]*SF[8] + P[3][15]*SF[6] + P[12][15]*SF[11] - P[10][15]*SPP[6] - (P[11][15]*q0)/2; - nextP[2][16] = P[2][16] + P[0][16]*SF[4] + P[1][16]*SF[8] + P[3][16]*SF[6] + P[12][16]*SF[11] - P[10][16]*SPP[6] - (P[11][16]*q0)/2; - nextP[2][17] = P[2][17] + P[0][17]*SF[4] + P[1][17]*SF[8] + P[3][17]*SF[6] + P[12][17]*SF[11] - P[10][17]*SPP[6] - (P[11][17]*q0)/2; - nextP[2][18] = P[2][18] + P[0][18]*SF[4] + P[1][18]*SF[8] + P[3][18]*SF[6] + P[12][18]*SF[11] - P[10][18]*SPP[6] - (P[11][18]*q0)/2; - nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2; - nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2; - nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2; - nextP[2][22] = P[2][22] + P[0][22]*SF[4] + P[1][22]*SF[8] + P[3][22]*SF[6] + P[12][22]*SF[11] - P[10][22]*SPP[6] - (P[11][22]*q0)/2; - nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2); - nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2; - nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2; - nextP[3][3] = P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[4]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[7]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SF[11]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[7]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2))/2; - nextP[3][4] = P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[4]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); - nextP[3][5] = P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); - nextP[3][6] = P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2 + SF[2]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[1]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); - nextP[3][7] = P[3][7] + P[0][7]*SF[5] + P[1][7]*SF[4] + P[2][7]*SF[7] - P[11][7]*SF[11] + P[10][7]*SPP[7] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2); - nextP[3][8] = P[3][8] + P[0][8]*SF[5] + P[1][8]*SF[4] + P[2][8]*SF[7] - P[11][8]*SF[11] + P[10][8]*SPP[7] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2); - nextP[3][9] = P[3][9] + P[0][9]*SF[5] + P[1][9]*SF[4] + P[2][9]*SF[7] - P[11][9]*SF[11] + P[10][9]*SPP[7] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2); - nextP[3][10] = P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2; - nextP[3][11] = P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2; - nextP[3][12] = P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2; - nextP[3][13] = P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2; - nextP[3][14] = P[3][14] + P[0][14]*SF[5] + P[1][14]*SF[4] + P[2][14]*SF[7] - P[11][14]*SF[11] + P[10][14]*SPP[7] - (P[12][14]*q0)/2; - nextP[3][15] = P[3][15] + P[0][15]*SF[5] + P[1][15]*SF[4] + P[2][15]*SF[7] - P[11][15]*SF[11] + P[10][15]*SPP[7] - (P[12][15]*q0)/2; - nextP[3][16] = P[3][16] + P[0][16]*SF[5] + P[1][16]*SF[4] + P[2][16]*SF[7] - P[11][16]*SF[11] + P[10][16]*SPP[7] - (P[12][16]*q0)/2; - nextP[3][17] = P[3][17] + P[0][17]*SF[5] + P[1][17]*SF[4] + P[2][17]*SF[7] - P[11][17]*SF[11] + P[10][17]*SPP[7] - (P[12][17]*q0)/2; - nextP[3][18] = P[3][18] + P[0][18]*SF[5] + P[1][18]*SF[4] + P[2][18]*SF[7] - P[11][18]*SF[11] + P[10][18]*SPP[7] - (P[12][18]*q0)/2; - nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2; - nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2; - nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2; - nextP[3][22] = P[3][22] + P[0][22]*SF[5] + P[1][22]*SF[4] + P[2][22]*SF[7] - P[11][22]*SF[11] + P[10][22]*SPP[7] - (P[12][22]*q0)/2; - nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]); - nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2; - nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2; - nextP[4][3] = P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4] + SF[5]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[4]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[7]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SF[11]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[7]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]))/2; - nextP[4][4] = P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[3]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[0]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SPP[2]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[4]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); - nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4] + SF[2]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[3]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[0]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[3]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); - nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4] + SF[2]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[1]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[0]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) - SPP[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); - nextP[4][7] = P[4][7] + P[0][7]*SF[3] + P[1][7]*SF[1] + P[2][7]*SPP[0] - P[3][7]*SPP[2] - P[13][7]*SPP[4] + dt*(P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4]); - nextP[4][8] = P[4][8] + P[0][8]*SF[3] + P[1][8]*SF[1] + P[2][8]*SPP[0] - P[3][8]*SPP[2] - P[13][8]*SPP[4] + dt*(P[4][5] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4]); - nextP[4][9] = P[4][9] + P[0][9]*SF[3] + P[1][9]*SF[1] + P[2][9]*SPP[0] - P[3][9]*SPP[2] - P[13][9]*SPP[4] + dt*(P[4][6] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4]); - nextP[4][10] = P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]; - nextP[4][11] = P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]; - nextP[4][12] = P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]; - nextP[4][13] = P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]; - nextP[4][14] = P[4][14] + P[0][14]*SF[3] + P[1][14]*SF[1] + P[2][14]*SPP[0] - P[3][14]*SPP[2] - P[13][14]*SPP[4]; - nextP[4][15] = P[4][15] + P[0][15]*SF[3] + P[1][15]*SF[1] + P[2][15]*SPP[0] - P[3][15]*SPP[2] - P[13][15]*SPP[4]; - nextP[4][16] = P[4][16] + P[0][16]*SF[3] + P[1][16]*SF[1] + P[2][16]*SPP[0] - P[3][16]*SPP[2] - P[13][16]*SPP[4]; - nextP[4][17] = P[4][17] + P[0][17]*SF[3] + P[1][17]*SF[1] + P[2][17]*SPP[0] - P[3][17]*SPP[2] - P[13][17]*SPP[4]; - nextP[4][18] = P[4][18] + P[0][18]*SF[3] + P[1][18]*SF[1] + P[2][18]*SPP[0] - P[3][18]*SPP[2] - P[13][18]*SPP[4]; - nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4]; - nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4]; - nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4]; - nextP[4][22] = P[4][22] + P[0][22]*SF[3] + P[1][22]*SF[1] + P[2][22]*SPP[0] - P[3][22]*SPP[2] - P[13][22]*SPP[4]; - nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]); - nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2; - nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2; - nextP[5][3] = P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3] + SF[5]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[4]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[7]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SF[11]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[7]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]))/2; - nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3] + SF[3]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[0]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SPP[2]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[4]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); - nextP[5][5] = P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[2]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[3]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[0]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[3]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); - nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3] + SF[2]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[1]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[0]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) - SPP[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); - nextP[5][7] = P[5][7] + P[0][7]*SF[2] + P[2][7]*SF[1] + P[3][7]*SF[3] - P[1][7]*SPP[0] + P[13][7]*SPP[3] + dt*(P[5][4] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3]); - nextP[5][8] = P[5][8] + P[0][8]*SF[2] + P[2][8]*SF[1] + P[3][8]*SF[3] - P[1][8]*SPP[0] + P[13][8]*SPP[3] + dt*(P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3]); - nextP[5][9] = P[5][9] + P[0][9]*SF[2] + P[2][9]*SF[1] + P[3][9]*SF[3] - P[1][9]*SPP[0] + P[13][9]*SPP[3] + dt*(P[5][6] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3]); - nextP[5][10] = P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]; - nextP[5][11] = P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]; - nextP[5][12] = P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]; - nextP[5][13] = P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]; - nextP[5][14] = P[5][14] + P[0][14]*SF[2] + P[2][14]*SF[1] + P[3][14]*SF[3] - P[1][14]*SPP[0] + P[13][14]*SPP[3]; - nextP[5][15] = P[5][15] + P[0][15]*SF[2] + P[2][15]*SF[1] + P[3][15]*SF[3] - P[1][15]*SPP[0] + P[13][15]*SPP[3]; - nextP[5][16] = P[5][16] + P[0][16]*SF[2] + P[2][16]*SF[1] + P[3][16]*SF[3] - P[1][16]*SPP[0] + P[13][16]*SPP[3]; - nextP[5][17] = P[5][17] + P[0][17]*SF[2] + P[2][17]*SF[1] + P[3][17]*SF[3] - P[1][17]*SPP[0] + P[13][17]*SPP[3]; - nextP[5][18] = P[5][18] + P[0][18]*SF[2] + P[2][18]*SF[1] + P[3][18]*SF[3] - P[1][18]*SPP[0] + P[13][18]*SPP[3]; - nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3]; - nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3]; - nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3]; - nextP[5][22] = P[5][22] + P[0][22]*SF[2] + P[2][22]*SF[1] + P[3][22]*SF[3] - P[1][22]*SPP[0] + P[13][22]*SPP[3]; - nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); - nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; - nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; - nextP[6][3] = P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[5]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[4]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[7]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SF[11]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; - nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[3]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[2]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[4]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); - nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[2]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[3]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[0]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[3]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); - nextP[6][6] = P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) - SPP[5]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]) + SF[2]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); - nextP[6][7] = P[6][7] + P[1][7]*SF[2] + P[3][7]*SF[1] + P[0][7]*SPP[0] - P[2][7]*SPP[1] - P[13][7]*SPP[5] + dt*(P[6][4] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*SPP[5]); - nextP[6][8] = P[6][8] + P[1][8]*SF[2] + P[3][8]*SF[1] + P[0][8]*SPP[0] - P[2][8]*SPP[1] - P[13][8]*SPP[5] + dt*(P[6][5] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*SPP[5]); - nextP[6][9] = P[6][9] + P[1][9]*SF[2] + P[3][9]*SF[1] + P[0][9]*SPP[0] - P[2][9]*SPP[1] - P[13][9]*SPP[5] + dt*(P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*SPP[5]); - nextP[6][10] = P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*SPP[5]; - nextP[6][11] = P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*SPP[5]; - nextP[6][12] = P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*SPP[5]; - nextP[6][13] = P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]; - nextP[6][14] = P[6][14] + P[1][14]*SF[2] + P[3][14]*SF[1] + P[0][14]*SPP[0] - P[2][14]*SPP[1] - P[13][14]*SPP[5]; - nextP[6][15] = P[6][15] + P[1][15]*SF[2] + P[3][15]*SF[1] + P[0][15]*SPP[0] - P[2][15]*SPP[1] - P[13][15]*SPP[5]; - nextP[6][16] = P[6][16] + P[1][16]*SF[2] + P[3][16]*SF[1] + P[0][16]*SPP[0] - P[2][16]*SPP[1] - P[13][16]*SPP[5]; - nextP[6][17] = P[6][17] + P[1][17]*SF[2] + P[3][17]*SF[1] + P[0][17]*SPP[0] - P[2][17]*SPP[1] - P[13][17]*SPP[5]; - nextP[6][18] = P[6][18] + P[1][18]*SF[2] + P[3][18]*SF[1] + P[0][18]*SPP[0] - P[2][18]*SPP[1] - P[13][18]*SPP[5]; - nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5]; - nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5]; - nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5]; - nextP[6][22] = P[6][22] + P[1][22]*SF[2] + P[3][22]*SF[1] + P[0][22]*SPP[0] - P[2][22]*SPP[1] - P[13][22]*SPP[5]; - nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt); - nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; - nextP[7][2] = P[7][2] + P[4][2]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; - nextP[7][3] = P[7][3] + P[4][3]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][1] + P[4][1]*dt) + SF[7]*(P[7][2] + P[4][2]*dt) - SF[11]*(P[7][11] + P[4][11]*dt) + SPP[7]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; - nextP[7][4] = P[7][4] + P[4][4]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[3]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt) - SPP[4]*(P[7][13] + P[4][13]*dt); - nextP[7][5] = P[7][5] + P[4][5]*dt + SF[2]*(P[7][0] + P[4][0]*dt) + SF[1]*(P[7][2] + P[4][2]*dt) + SF[3]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt) + SPP[3]*(P[7][13] + P[4][13]*dt); - nextP[7][6] = P[7][6] + P[4][6]*dt + SF[2]*(P[7][1] + P[4][1]*dt) + SF[1]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt) - SPP[5]*(P[7][13] + P[4][13]*dt); - nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); - nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); - nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); - nextP[7][10] = P[7][10] + P[4][10]*dt; - nextP[7][11] = P[7][11] + P[4][11]*dt; - nextP[7][12] = P[7][12] + P[4][12]*dt; - nextP[7][13] = P[7][13] + P[4][13]*dt; - nextP[7][14] = P[7][14] + P[4][14]*dt; - nextP[7][15] = P[7][15] + P[4][15]*dt; - nextP[7][16] = P[7][16] + P[4][16]*dt; - nextP[7][17] = P[7][17] + P[4][17]*dt; - nextP[7][18] = P[7][18] + P[4][18]*dt; - nextP[7][19] = P[7][19] + P[4][19]*dt; - nextP[7][20] = P[7][20] + P[4][20]*dt; - nextP[7][21] = P[7][21] + P[4][21]*dt; - nextP[7][22] = P[7][22] + P[4][22]*dt; - nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt); - nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; - nextP[8][2] = P[8][2] + P[5][2]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; - nextP[8][3] = P[8][3] + P[5][3]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][1] + P[5][1]*dt) + SF[7]*(P[8][2] + P[5][2]*dt) - SF[11]*(P[8][11] + P[5][11]*dt) + SPP[7]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; - nextP[8][4] = P[8][4] + P[5][4]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[3]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt) - SPP[4]*(P[8][13] + P[5][13]*dt); - nextP[8][5] = P[8][5] + P[5][5]*dt + SF[2]*(P[8][0] + P[5][0]*dt) + SF[1]*(P[8][2] + P[5][2]*dt) + SF[3]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt) + SPP[3]*(P[8][13] + P[5][13]*dt); - nextP[8][6] = P[8][6] + P[5][6]*dt + SF[2]*(P[8][1] + P[5][1]*dt) + SF[1]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt) - SPP[5]*(P[8][13] + P[5][13]*dt); - nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); - nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); - nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); - nextP[8][10] = P[8][10] + P[5][10]*dt; - nextP[8][11] = P[8][11] + P[5][11]*dt; - nextP[8][12] = P[8][12] + P[5][12]*dt; - nextP[8][13] = P[8][13] + P[5][13]*dt; - nextP[8][14] = P[8][14] + P[5][14]*dt; - nextP[8][15] = P[8][15] + P[5][15]*dt; - nextP[8][16] = P[8][16] + P[5][16]*dt; - nextP[8][17] = P[8][17] + P[5][17]*dt; - nextP[8][18] = P[8][18] + P[5][18]*dt; - nextP[8][19] = P[8][19] + P[5][19]*dt; - nextP[8][20] = P[8][20] + P[5][20]*dt; - nextP[8][21] = P[8][21] + P[5][21]*dt; - nextP[8][22] = P[8][22] + P[5][22]*dt; - nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt); - nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; - nextP[9][2] = P[9][2] + P[6][2]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; - nextP[9][3] = P[9][3] + P[6][3]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][1] + P[6][1]*dt) + SF[7]*(P[9][2] + P[6][2]*dt) - SF[11]*(P[9][11] + P[6][11]*dt) + SPP[7]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; - nextP[9][4] = P[9][4] + P[6][4]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[3]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt) - SPP[4]*(P[9][13] + P[6][13]*dt); - nextP[9][5] = P[9][5] + P[6][5]*dt + SF[2]*(P[9][0] + P[6][0]*dt) + SF[1]*(P[9][2] + P[6][2]*dt) + SF[3]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt) + SPP[3]*(P[9][13] + P[6][13]*dt); - nextP[9][6] = P[9][6] + P[6][6]*dt + SF[2]*(P[9][1] + P[6][1]*dt) + SF[1]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt) - SPP[5]*(P[9][13] + P[6][13]*dt); - nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); - nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); - nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); - nextP[9][10] = P[9][10] + P[6][10]*dt; - nextP[9][11] = P[9][11] + P[6][11]*dt; - nextP[9][12] = P[9][12] + P[6][12]*dt; - nextP[9][13] = P[9][13] + P[6][13]*dt; - nextP[9][14] = P[9][14] + P[6][14]*dt; - nextP[9][15] = P[9][15] + P[6][15]*dt; - nextP[9][16] = P[9][16] + P[6][16]*dt; - nextP[9][17] = P[9][17] + P[6][17]*dt; - nextP[9][18] = P[9][18] + P[6][18]*dt; - nextP[9][19] = P[9][19] + P[6][19]*dt; - nextP[9][20] = P[9][20] + P[6][20]*dt; - nextP[9][21] = P[9][21] + P[6][21]*dt; - nextP[9][22] = P[9][22] + P[6][22]*dt; - nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6]; - nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2; - nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2; - nextP[10][3] = P[10][3] + P[10][0]*SF[5] + P[10][1]*SF[4] + P[10][2]*SF[7] - P[10][11]*SF[11] + P[10][10]*SPP[7] - (P[10][12]*q0)/2; - nextP[10][4] = P[10][4] + P[10][1]*SF[1] + P[10][0]*SF[3] + P[10][2]*SPP[0] - P[10][3]*SPP[2] - P[10][13]*SPP[4]; - nextP[10][5] = P[10][5] + P[10][0]*SF[2] + P[10][2]*SF[1] + P[10][3]*SF[3] - P[10][1]*SPP[0] + P[10][13]*SPP[3]; - nextP[10][6] = P[10][6] + P[10][1]*SF[2] + P[10][3]*SF[1] + P[10][0]*SPP[0] - P[10][2]*SPP[1] - P[10][13]*SPP[5]; - nextP[10][7] = P[10][7] + P[10][4]*dt; - nextP[10][8] = P[10][8] + P[10][5]*dt; - nextP[10][9] = P[10][9] + P[10][6]*dt; - nextP[10][10] = P[10][10]; - nextP[10][11] = P[10][11]; - nextP[10][12] = P[10][12]; - nextP[10][13] = P[10][13]; - nextP[10][14] = P[10][14]; - nextP[10][15] = P[10][15]; - nextP[10][16] = P[10][16]; - nextP[10][17] = P[10][17]; - nextP[10][18] = P[10][18]; - nextP[10][19] = P[10][19]; - nextP[10][20] = P[10][20]; - nextP[10][21] = P[10][21]; - nextP[10][22] = P[10][22]; - nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6]; - nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2; - nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2; - nextP[11][3] = P[11][3] + P[11][0]*SF[5] + P[11][1]*SF[4] + P[11][2]*SF[7] - P[11][11]*SF[11] + P[11][10]*SPP[7] - (P[11][12]*q0)/2; - nextP[11][4] = P[11][4] + P[11][1]*SF[1] + P[11][0]*SF[3] + P[11][2]*SPP[0] - P[11][3]*SPP[2] - P[11][13]*SPP[4]; - nextP[11][5] = P[11][5] + P[11][0]*SF[2] + P[11][2]*SF[1] + P[11][3]*SF[3] - P[11][1]*SPP[0] + P[11][13]*SPP[3]; - nextP[11][6] = P[11][6] + P[11][1]*SF[2] + P[11][3]*SF[1] + P[11][0]*SPP[0] - P[11][2]*SPP[1] - P[11][13]*SPP[5]; - nextP[11][7] = P[11][7] + P[11][4]*dt; - nextP[11][8] = P[11][8] + P[11][5]*dt; - nextP[11][9] = P[11][9] + P[11][6]*dt; - nextP[11][10] = P[11][10]; - nextP[11][11] = P[11][11]; - nextP[11][12] = P[11][12]; - nextP[11][13] = P[11][13]; - nextP[11][14] = P[11][14]; - nextP[11][15] = P[11][15]; - nextP[11][16] = P[11][16]; - nextP[11][17] = P[11][17]; - nextP[11][18] = P[11][18]; - nextP[11][19] = P[11][19]; - nextP[11][20] = P[11][20]; - nextP[11][21] = P[11][21]; - nextP[11][22] = P[11][22]; - nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6]; - nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2; - nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2; - nextP[12][3] = P[12][3] + P[12][0]*SF[5] + P[12][1]*SF[4] + P[12][2]*SF[7] - P[12][11]*SF[11] + P[12][10]*SPP[7] - (P[12][12]*q0)/2; - nextP[12][4] = P[12][4] + P[12][1]*SF[1] + P[12][0]*SF[3] + P[12][2]*SPP[0] - P[12][3]*SPP[2] - P[12][13]*SPP[4]; - nextP[12][5] = P[12][5] + P[12][0]*SF[2] + P[12][2]*SF[1] + P[12][3]*SF[3] - P[12][1]*SPP[0] + P[12][13]*SPP[3]; - nextP[12][6] = P[12][6] + P[12][1]*SF[2] + P[12][3]*SF[1] + P[12][0]*SPP[0] - P[12][2]*SPP[1] - P[12][13]*SPP[5]; - nextP[12][7] = P[12][7] + P[12][4]*dt; - nextP[12][8] = P[12][8] + P[12][5]*dt; - nextP[12][9] = P[12][9] + P[12][6]*dt; - nextP[12][10] = P[12][10]; - nextP[12][11] = P[12][11]; - nextP[12][12] = P[12][12]; - nextP[12][13] = P[12][13]; - nextP[12][14] = P[12][14]; - nextP[12][15] = P[12][15]; - nextP[12][16] = P[12][16]; - nextP[12][17] = P[12][17]; - nextP[12][18] = P[12][18]; - nextP[12][19] = P[12][19]; - nextP[12][20] = P[12][20]; - nextP[12][21] = P[12][21]; - nextP[12][22] = P[12][22]; - nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6]; - nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2; - nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2; - nextP[13][3] = P[13][3] + P[13][0]*SF[5] + P[13][1]*SF[4] + P[13][2]*SF[7] - P[13][11]*SF[11] + P[13][10]*SPP[7] - (P[13][12]*q0)/2; - nextP[13][4] = P[13][4] + P[13][1]*SF[1] + P[13][0]*SF[3] + P[13][2]*SPP[0] - P[13][3]*SPP[2] - P[13][13]*SPP[4]; - nextP[13][5] = P[13][5] + P[13][0]*SF[2] + P[13][2]*SF[1] + P[13][3]*SF[3] - P[13][1]*SPP[0] + P[13][13]*SPP[3]; - nextP[13][6] = P[13][6] + P[13][1]*SF[2] + P[13][3]*SF[1] + P[13][0]*SPP[0] - P[13][2]*SPP[1] - P[13][13]*SPP[5]; - nextP[13][7] = P[13][7] + P[13][4]*dt; - nextP[13][8] = P[13][8] + P[13][5]*dt; - nextP[13][9] = P[13][9] + P[13][6]*dt; - nextP[13][10] = P[13][10]; - nextP[13][11] = P[13][11]; - nextP[13][12] = P[13][12]; - nextP[13][13] = P[13][13]; - nextP[13][14] = P[13][14]; - nextP[13][15] = P[13][15]; - nextP[13][16] = P[13][16]; - nextP[13][17] = P[13][17]; - nextP[13][18] = P[13][18]; - nextP[13][19] = P[13][19]; - nextP[13][20] = P[13][20]; - nextP[13][21] = P[13][21]; - nextP[13][22] = P[13][22]; - nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6]; - nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2; - nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2; - nextP[14][3] = P[14][3] + P[14][0]*SF[5] + P[14][1]*SF[4] + P[14][2]*SF[7] - P[14][11]*SF[11] + P[14][10]*SPP[7] - (P[14][12]*q0)/2; - nextP[14][4] = P[14][4] + P[14][1]*SF[1] + P[14][0]*SF[3] + P[14][2]*SPP[0] - P[14][3]*SPP[2] - P[14][13]*SPP[4]; - nextP[14][5] = P[14][5] + P[14][0]*SF[2] + P[14][2]*SF[1] + P[14][3]*SF[3] - P[14][1]*SPP[0] + P[14][13]*SPP[3]; - nextP[14][6] = P[14][6] + P[14][1]*SF[2] + P[14][3]*SF[1] + P[14][0]*SPP[0] - P[14][2]*SPP[1] - P[14][13]*SPP[5]; - nextP[14][7] = P[14][7] + P[14][4]*dt; - nextP[14][8] = P[14][8] + P[14][5]*dt; - nextP[14][9] = P[14][9] + P[14][6]*dt; - nextP[14][10] = P[14][10]; - nextP[14][11] = P[14][11]; - nextP[14][12] = P[14][12]; - nextP[14][13] = P[14][13]; - nextP[14][14] = P[14][14]; - nextP[14][15] = P[14][15]; - nextP[14][16] = P[14][16]; - nextP[14][17] = P[14][17]; - nextP[14][18] = P[14][18]; - nextP[14][19] = P[14][19]; - nextP[14][20] = P[14][20]; - nextP[14][21] = P[14][21]; - nextP[14][22] = P[14][22]; - nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6]; - nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2; - nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2; - nextP[15][3] = P[15][3] + P[15][0]*SF[5] + P[15][1]*SF[4] + P[15][2]*SF[7] - P[15][11]*SF[11] + P[15][10]*SPP[7] - (P[15][12]*q0)/2; - nextP[15][4] = P[15][4] + P[15][1]*SF[1] + P[15][0]*SF[3] + P[15][2]*SPP[0] - P[15][3]*SPP[2] - P[15][13]*SPP[4]; - nextP[15][5] = P[15][5] + P[15][0]*SF[2] + P[15][2]*SF[1] + P[15][3]*SF[3] - P[15][1]*SPP[0] + P[15][13]*SPP[3]; - nextP[15][6] = P[15][6] + P[15][1]*SF[2] + P[15][3]*SF[1] + P[15][0]*SPP[0] - P[15][2]*SPP[1] - P[15][13]*SPP[5]; - nextP[15][7] = P[15][7] + P[15][4]*dt; - nextP[15][8] = P[15][8] + P[15][5]*dt; - nextP[15][9] = P[15][9] + P[15][6]*dt; - nextP[15][10] = P[15][10]; - nextP[15][11] = P[15][11]; - nextP[15][12] = P[15][12]; - nextP[15][13] = P[15][13]; - nextP[15][14] = P[15][14]; - nextP[15][15] = P[15][15]; - nextP[15][16] = P[15][16]; - nextP[15][17] = P[15][17]; - nextP[15][18] = P[15][18]; - nextP[15][19] = P[15][19]; - nextP[15][20] = P[15][20]; - nextP[15][21] = P[15][21]; - nextP[15][22] = P[15][22]; - nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6]; - nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2; - nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2; - nextP[16][3] = P[16][3] + P[16][0]*SF[5] + P[16][1]*SF[4] + P[16][2]*SF[7] - P[16][11]*SF[11] + P[16][10]*SPP[7] - (P[16][12]*q0)/2; - nextP[16][4] = P[16][4] + P[16][1]*SF[1] + P[16][0]*SF[3] + P[16][2]*SPP[0] - P[16][3]*SPP[2] - P[16][13]*SPP[4]; - nextP[16][5] = P[16][5] + P[16][0]*SF[2] + P[16][2]*SF[1] + P[16][3]*SF[3] - P[16][1]*SPP[0] + P[16][13]*SPP[3]; - nextP[16][6] = P[16][6] + P[16][1]*SF[2] + P[16][3]*SF[1] + P[16][0]*SPP[0] - P[16][2]*SPP[1] - P[16][13]*SPP[5]; - nextP[16][7] = P[16][7] + P[16][4]*dt; - nextP[16][8] = P[16][8] + P[16][5]*dt; - nextP[16][9] = P[16][9] + P[16][6]*dt; - nextP[16][10] = P[16][10]; - nextP[16][11] = P[16][11]; - nextP[16][12] = P[16][12]; - nextP[16][13] = P[16][13]; - nextP[16][14] = P[16][14]; - nextP[16][15] = P[16][15]; - nextP[16][16] = P[16][16]; - nextP[16][17] = P[16][17]; - nextP[16][18] = P[16][18]; - nextP[16][19] = P[16][19]; - nextP[16][20] = P[16][20]; - nextP[16][21] = P[16][21]; - nextP[16][22] = P[16][22]; - nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6]; - nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2; - nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2; - nextP[17][3] = P[17][3] + P[17][0]*SF[5] + P[17][1]*SF[4] + P[17][2]*SF[7] - P[17][11]*SF[11] + P[17][10]*SPP[7] - (P[17][12]*q0)/2; - nextP[17][4] = P[17][4] + P[17][1]*SF[1] + P[17][0]*SF[3] + P[17][2]*SPP[0] - P[17][3]*SPP[2] - P[17][13]*SPP[4]; - nextP[17][5] = P[17][5] + P[17][0]*SF[2] + P[17][2]*SF[1] + P[17][3]*SF[3] - P[17][1]*SPP[0] + P[17][13]*SPP[3]; - nextP[17][6] = P[17][6] + P[17][1]*SF[2] + P[17][3]*SF[1] + P[17][0]*SPP[0] - P[17][2]*SPP[1] - P[17][13]*SPP[5]; - nextP[17][7] = P[17][7] + P[17][4]*dt; - nextP[17][8] = P[17][8] + P[17][5]*dt; - nextP[17][9] = P[17][9] + P[17][6]*dt; - nextP[17][10] = P[17][10]; - nextP[17][11] = P[17][11]; - nextP[17][12] = P[17][12]; - nextP[17][13] = P[17][13]; - nextP[17][14] = P[17][14]; - nextP[17][15] = P[17][15]; - nextP[17][16] = P[17][16]; - nextP[17][17] = P[17][17]; - nextP[17][18] = P[17][18]; - nextP[17][19] = P[17][19]; - nextP[17][20] = P[17][20]; - nextP[17][21] = P[17][21]; - nextP[17][22] = P[17][22]; - nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6]; - nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2; - nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2; - nextP[18][3] = P[18][3] + P[18][0]*SF[5] + P[18][1]*SF[4] + P[18][2]*SF[7] - P[18][11]*SF[11] + P[18][10]*SPP[7] - (P[18][12]*q0)/2; - nextP[18][4] = P[18][4] + P[18][1]*SF[1] + P[18][0]*SF[3] + P[18][2]*SPP[0] - P[18][3]*SPP[2] - P[18][13]*SPP[4]; - nextP[18][5] = P[18][5] + P[18][0]*SF[2] + P[18][2]*SF[1] + P[18][3]*SF[3] - P[18][1]*SPP[0] + P[18][13]*SPP[3]; - nextP[18][6] = P[18][6] + P[18][1]*SF[2] + P[18][3]*SF[1] + P[18][0]*SPP[0] - P[18][2]*SPP[1] - P[18][13]*SPP[5]; - nextP[18][7] = P[18][7] + P[18][4]*dt; - nextP[18][8] = P[18][8] + P[18][5]*dt; - nextP[18][9] = P[18][9] + P[18][6]*dt; - nextP[18][10] = P[18][10]; - nextP[18][11] = P[18][11]; - nextP[18][12] = P[18][12]; - nextP[18][13] = P[18][13]; - nextP[18][14] = P[18][14]; - nextP[18][15] = P[18][15]; - nextP[18][16] = P[18][16]; - nextP[18][17] = P[18][17]; - nextP[18][18] = P[18][18]; - nextP[18][19] = P[18][19]; - nextP[18][20] = P[18][20]; - nextP[18][21] = P[18][21]; - nextP[18][22] = P[18][22]; - nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6]; - nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2; - nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2; - nextP[19][3] = P[19][3] + P[19][0]*SF[5] + P[19][1]*SF[4] + P[19][2]*SF[7] - P[19][11]*SF[11] + P[19][10]*SPP[7] - (P[19][12]*q0)/2; - nextP[19][4] = P[19][4] + P[19][1]*SF[1] + P[19][0]*SF[3] + P[19][2]*SPP[0] - P[19][3]*SPP[2] - P[19][13]*SPP[4]; - nextP[19][5] = P[19][5] + P[19][0]*SF[2] + P[19][2]*SF[1] + P[19][3]*SF[3] - P[19][1]*SPP[0] + P[19][13]*SPP[3]; - nextP[19][6] = P[19][6] + P[19][1]*SF[2] + P[19][3]*SF[1] + P[19][0]*SPP[0] - P[19][2]*SPP[1] - P[19][13]*SPP[5]; - nextP[19][7] = P[19][7] + P[19][4]*dt; - nextP[19][8] = P[19][8] + P[19][5]*dt; - nextP[19][9] = P[19][9] + P[19][6]*dt; - nextP[19][10] = P[19][10]; - nextP[19][11] = P[19][11]; - nextP[19][12] = P[19][12]; - nextP[19][13] = P[19][13]; - nextP[19][14] = P[19][14]; - nextP[19][15] = P[19][15]; - nextP[19][16] = P[19][16]; - nextP[19][17] = P[19][17]; - nextP[19][18] = P[19][18]; - nextP[19][19] = P[19][19]; - nextP[19][20] = P[19][20]; - nextP[19][21] = P[19][21]; - nextP[19][22] = P[19][22]; - nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6]; - nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2; - nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2; - nextP[20][3] = P[20][3] + P[20][0]*SF[5] + P[20][1]*SF[4] + P[20][2]*SF[7] - P[20][11]*SF[11] + P[20][10]*SPP[7] - (P[20][12]*q0)/2; - nextP[20][4] = P[20][4] + P[20][1]*SF[1] + P[20][0]*SF[3] + P[20][2]*SPP[0] - P[20][3]*SPP[2] - P[20][13]*SPP[4]; - nextP[20][5] = P[20][5] + P[20][0]*SF[2] + P[20][2]*SF[1] + P[20][3]*SF[3] - P[20][1]*SPP[0] + P[20][13]*SPP[3]; - nextP[20][6] = P[20][6] + P[20][1]*SF[2] + P[20][3]*SF[1] + P[20][0]*SPP[0] - P[20][2]*SPP[1] - P[20][13]*SPP[5]; - nextP[20][7] = P[20][7] + P[20][4]*dt; - nextP[20][8] = P[20][8] + P[20][5]*dt; - nextP[20][9] = P[20][9] + P[20][6]*dt; - nextP[20][10] = P[20][10]; - nextP[20][11] = P[20][11]; - nextP[20][12] = P[20][12]; - nextP[20][13] = P[20][13]; - nextP[20][14] = P[20][14]; - nextP[20][15] = P[20][15]; - nextP[20][16] = P[20][16]; - nextP[20][17] = P[20][17]; - nextP[20][18] = P[20][18]; - nextP[20][19] = P[20][19]; - nextP[20][20] = P[20][20]; - nextP[20][21] = P[20][21]; - nextP[20][22] = P[20][22]; - nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6]; - nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2; - nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2; - nextP[21][3] = P[21][3] + P[21][0]*SF[5] + P[21][1]*SF[4] + P[21][2]*SF[7] - P[21][11]*SF[11] + P[21][10]*SPP[7] - (P[21][12]*q0)/2; - nextP[21][4] = P[21][4] + P[21][1]*SF[1] + P[21][0]*SF[3] + P[21][2]*SPP[0] - P[21][3]*SPP[2] - P[21][13]*SPP[4]; - nextP[21][5] = P[21][5] + P[21][0]*SF[2] + P[21][2]*SF[1] + P[21][3]*SF[3] - P[21][1]*SPP[0] + P[21][13]*SPP[3]; - nextP[21][6] = P[21][6] + P[21][1]*SF[2] + P[21][3]*SF[1] + P[21][0]*SPP[0] - P[21][2]*SPP[1] - P[21][13]*SPP[5]; - nextP[21][7] = P[21][7] + P[21][4]*dt; - nextP[21][8] = P[21][8] + P[21][5]*dt; - nextP[21][9] = P[21][9] + P[21][6]*dt; - nextP[21][10] = P[21][10]; - nextP[21][11] = P[21][11]; - nextP[21][12] = P[21][12]; - nextP[21][13] = P[21][13]; - nextP[21][14] = P[21][14]; - nextP[21][15] = P[21][15]; - nextP[21][16] = P[21][16]; - nextP[21][17] = P[21][17]; - nextP[21][18] = P[21][18]; - nextP[21][19] = P[21][19]; - nextP[21][20] = P[21][20]; - nextP[21][21] = P[21][21]; - nextP[21][22] = P[21][22]; - nextP[22][0] = P[22][0] + P[22][1]*SF[7] + P[22][2]*SF[9] + P[22][3]*SF[8] + P[22][10]*SF[11] + P[22][11]*SPP[7] + P[22][12]*SPP[6]; - nextP[22][1] = P[22][1] + P[22][0]*SF[6] + P[22][2]*SF[5] + P[22][3]*SF[9] + P[22][11]*SPP[6] - P[22][12]*SPP[7] - (P[22][10]*q0)/2; - nextP[22][2] = P[22][2] + P[22][0]*SF[4] + P[22][1]*SF[8] + P[22][3]*SF[6] + P[22][12]*SF[11] - P[22][10]*SPP[6] - (P[22][11]*q0)/2; - nextP[22][3] = P[22][3] + P[22][0]*SF[5] + P[22][1]*SF[4] + P[22][2]*SF[7] - P[22][11]*SF[11] + P[22][10]*SPP[7] - (P[22][12]*q0)/2; - nextP[22][4] = P[22][4] + P[22][1]*SF[1] + P[22][0]*SF[3] + P[22][2]*SPP[0] - P[22][3]*SPP[2] - P[22][13]*SPP[4]; - nextP[22][5] = P[22][5] + P[22][0]*SF[2] + P[22][2]*SF[1] + P[22][3]*SF[3] - P[22][1]*SPP[0] + P[22][13]*SPP[3]; - nextP[22][6] = P[22][6] + P[22][1]*SF[2] + P[22][3]*SF[1] + P[22][0]*SPP[0] - P[22][2]*SPP[1] - P[22][13]*SPP[5]; - nextP[22][7] = P[22][7] + P[22][4]*dt; - nextP[22][8] = P[22][8] + P[22][5]*dt; - nextP[22][9] = P[22][9] + P[22][6]*dt; - nextP[22][10] = P[22][10]; - nextP[22][11] = P[22][11]; - nextP[22][12] = P[22][12]; - nextP[22][13] = P[22][13]; - nextP[22][14] = P[22][14]; - nextP[22][15] = P[22][15]; - nextP[22][16] = P[22][16]; - nextP[22][17] = P[22][17]; - nextP[22][18] = P[22][18]; - nextP[22][19] = P[22][19]; - nextP[22][20] = P[22][20]; - nextP[22][21] = P[22][21]; - nextP[22][22] = P[22][22]; - - for (unsigned i = 0; i < n_states; i++) - { - nextP[i][i] = nextP[i][i] + processNoise[i]; - } - - // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by - // setting the coresponding covariance terms to zero. - if (onGround || !useCompass) - { - zeroRows(nextP,16,21); - zeroCols(nextP,16,21); - } - - // If on ground or not using airspeed sensing, inhibit wind velocity - // covariance growth. - if (onGround || !useAirspeed) - { - zeroRows(nextP,14,15); - zeroCols(nextP,14,15); - } - - // If on ground, inhibit terrain offset updates by - // setting the coresponding covariance terms to zero. - if (onGround) - { - zeroRows(nextP,22,22); - zeroCols(nextP,22,22); - } - - // If the total position variance exceds 1E6 (1000m), then stop covariance - // growth by setting the predicted to the previous values - // This prevent an ill conditioned matrix from occurring for long periods - // without GPS - if ((P[7][7] + P[8][8]) > 1E6f) - { - for (uint8_t i=7; i<=8; i++) - { - for (unsigned j = 0; j < n_states; j++) - { - nextP[i][j] = P[i][j]; - nextP[j][i] = P[j][i]; - } - } - } - - if (onGround || staticMode) { - // copy the portion of the variances we want to - // propagate - for (unsigned i = 0; i <= 13; i++) { - P[i][i] = nextP[i][i]; - - // force symmetry for observable states - // force zero for non-observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - if ((i > 13) || (j > 13)) { - P[i][j] = 0.0f; - } else { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - } - P[j][i] = P[i][j]; - } - } - } - - } else { - - // Copy covariance - for (unsigned i = 0; i < n_states; i++) { - P[i][i] = nextP[i][i]; - } - - // force symmetry for observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - P[j][i] = P[i][j]; - } - } - - } - - ConstrainVariances(); -} - -void AttPosEKF::FuseVelposNED() -{ - -// declare variables used by fault isolation logic - uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure - uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available - uint32_t hgtRetryTime = 5000; // height measurement retry time - uint32_t horizRetryTime; - -// declare variables used to check measurement errors - float velInnov[3] = {0.0f,0.0f,0.0f}; - float posInnov[2] = {0.0f,0.0f}; - float hgtInnov = 0.0f; - -// declare variables used to control access to arrays - bool fuseData[6] = {false,false,false,false,false,false}; - uint8_t stateIndex; - uint8_t obsIndex; - uint8_t indexLimit; - -// declare variables used by state and covariance update calculations - float velErr; - float posErr; - float R_OBS[6]; - float observation[6]; - float SK; - float quatMag; - -// Perform sequential fusion of GPS measurements. This assumes that the -// errors in the different velocity and position components are -// uncorrelated which is not true, however in the absence of covariance -// data from the GPS receiver it is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion - if (fuseVelData || fusePosData || fuseHgtData) - { - // set the GPS data timeout depending on whether airspeed data is present - if (useAirspeed) horizRetryTime = gpsRetryTime; - else horizRetryTime = gpsRetryTimeNoTAS; - - // Form the observation vector - for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; - for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; - observation[5] = -(hgtMea); - - // Estimate the GPS Velocity, GPS horiz position and height measurement variances. - velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring - posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring - R_OBS[0] = sq(vneSigma) + sq(velErr); - R_OBS[1] = R_OBS[0]; - R_OBS[2] = sq(vdSigma) + sq(velErr); - R_OBS[3] = sq(posNeSigma) + sq(posErr); - R_OBS[4] = R_OBS[3]; - R_OBS[5] = sq(posDSigma) + sq(posErr); - - // Set innovation variances to zero default - for (uint8_t i = 0; i<=5; i++) - { - varInnovVelPos[i] = 0.0f; - } - // calculate innovations and check GPS data validity using an innovation consistency check - if (fuseVelData) - { - // test velocity measurements - uint8_t imax = 2; - if (fusionModeGPS == 1) imax = 1; - for (uint8_t i = 0; i<=imax; i++) - { - velInnov[i] = statesAtVelTime[i+4] - velNED[i]; - stateIndex = 4 + i; - varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; - } - // apply a 5-sigma threshold - current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); - current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; - if (current_ekf_state.velHealth || current_ekf_state.velTimeout) - { - current_ekf_state.velHealth = true; - current_ekf_state.velFailTime = millis(); - } - else - { - current_ekf_state.velHealth = false; - } - } - if (fusePosData) - { - // test horizontal position measurements - posInnov[0] = statesAtPosTime[7] - posNE[0]; - posInnov[1] = statesAtPosTime[8] - posNE[1]; - varInnovVelPos[3] = P[7][7] + R_OBS[3]; - varInnovVelPos[4] = P[8][8] + R_OBS[4]; - // apply a 10-sigma threshold - current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); - current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; - if (current_ekf_state.posHealth || current_ekf_state.posTimeout) - { - current_ekf_state.posHealth = true; - current_ekf_state.posFailTime = millis(); - } - else - { - current_ekf_state.posHealth = false; - } - } - // test height measurements - if (fuseHgtData) - { - hgtInnov = statesAtHgtTime[9] + hgtMea; - varInnovVelPos[5] = P[9][9] + R_OBS[5]; - // apply a 10-sigma threshold - current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; - current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; - if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) - { - current_ekf_state.hgtHealth = true; - current_ekf_state.hgtFailTime = millis(); - } - else - { - current_ekf_state.hgtHealth = false; - } - } - // Set range for sequential fusion of velocity and position measurements depending - // on which data is available and its health - if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) - { - fuseData[0] = true; - fuseData[1] = true; - fuseData[2] = true; - } - if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) - { - fuseData[0] = true; - fuseData[1] = true; - } - if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) - { - fuseData[3] = true; - fuseData[4] = true; - } - if (fuseHgtData && current_ekf_state.hgtHealth) - { - fuseData[5] = true; - } - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = 22; - } - else - { - indexLimit = 13; - } - // Fuse measurements sequentially - for (obsIndex=0; obsIndex<=5; obsIndex++) - { - if (fuseData[obsIndex]) - { - stateIndex = 4 + obsIndex; - // Calculate the measurement innovation, using states from a - // different time coordinate if fusing height data - if (obsIndex >= 0 && obsIndex <= 2) - { - innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; - } - else if (obsIndex == 3 || obsIndex == 4) - { - innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; - } - else if (obsIndex == 5) - { - innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; - } - // Calculate the Kalman Gain - // Calculate innovation variances - also used for data logging - varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; - SK = 1.0/varInnovVelPos[obsIndex]; - for (uint8_t i= 0; i<=indexLimit; i++) - { - Kfusion[i] = P[i][stateIndex]*SK; - } - - // Don't update Z accel bias state unless using a height observation (GPS velocities can be biased) - if (obsIndex != 5) { - Kfusion[13] = 0; - } - - // Calculate state corrections and re-normalise the quaternions - for (uint8_t i = 0; i<=indexLimit; i++) - { - states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; - } - quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12f) // divide by 0 protection - { - for (uint8_t i = 0; i<=3; i++) - { - states[i] = states[i] / quatMag; - } - } - // Update the covariance - take advantage of direct observation of a - // single state at index = stateIndex to reduce computations - // Optimised implementation of standard equation P = (I - K*H)*P; - for (uint8_t i= 0; i<=indexLimit; i++) - { - for (uint8_t j= 0; j<=indexLimit; j++) - { - KHP[i][j] = Kfusion[i] * P[stateIndex][j]; - } - } - for (uint8_t i= 0; i<=indexLimit; i++) - { - for (uint8_t j= 0; j<=indexLimit; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - } - } - - ForceSymmetry(); - ConstrainVariances(); - -} - -void AttPosEKF::FuseMagnetometer() -{ - - float &q0 = magstate.q0; - float &q1 = magstate.q1; - float &q2 = magstate.q2; - float &q3 = magstate.q3; - float &magN = magstate.magN; - float &magE = magstate.magE; - float &magD = magstate.magD; - float &magXbias = magstate.magXbias; - float &magYbias = magstate.magYbias; - float &magZbias = magstate.magZbias; - unsigned &obsIndex = magstate.obsIndex; - Mat3f &DCM = magstate.DCM; - float *MagPred = &magstate.MagPred[0]; - float &R_MAG = magstate.R_MAG; - float *SH_MAG = &magstate.SH_MAG[0]; - - float SK_MX[6]; - float SK_MY[5]; - float SK_MZ[6]; - float H_MAG[n_states]; - for (uint8_t i = 0; i < n_states; i++) { - H_MAG[i] = 0.0f; - } - unsigned indexLimit; - -// Perform sequential fusion of Magnetometer measurements. -// This assumes that the errors in the different components are -// uncorrelated which is not true, however in the absence of covariance -// data fit is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion - if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) - { - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = n_states; - } - else - { - indexLimit = 13 + 1; - } - - // Sequential fusion of XYZ components to spread processing load across - // three prediction time steps. - - // Calculate observation jacobians and Kalman gains - if (fuseMagData) - { - // Copy required states to local variable names - q0 = statesAtMagMeasTime[0]; - q1 = statesAtMagMeasTime[1]; - q2 = statesAtMagMeasTime[2]; - q3 = statesAtMagMeasTime[3]; - magN = statesAtMagMeasTime[16]; - magE = statesAtMagMeasTime[17]; - magD = statesAtMagMeasTime[18]; - magXbias = statesAtMagMeasTime[19]; - magYbias = statesAtMagMeasTime[20]; - magZbias = statesAtMagMeasTime[21]; - - // rotate predicted earth components into body axes and calculate - // predicted measurments - DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3; - DCM.x.y = 2*(q1*q2 + q0*q3); - DCM.x.z = 2*(q1*q3-q0*q2); - DCM.y.x = 2*(q1*q2 - q0*q3); - DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3; - DCM.y.z = 2*(q2*q3 + q0*q1); - DCM.z.x = 2*(q1*q3 + q0*q2); - DCM.z.y = 2*(q2*q3 - q0*q1); - DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3; - MagPred[0] = DCM.x.x*magN + DCM.x.y*magE + DCM.x.z*magD + magXbias; - MagPred[1] = DCM.y.x*magN + DCM.y.y*magE + DCM.y.z*magD + magYbias; - MagPred[2] = DCM.z.x*magN + DCM.z.y*magE + DCM.z.z*magD + magZbias; - - // scale magnetometer observation error with total angular rate - R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU); - - // Calculate observation jacobians - SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; - SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; - SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; - SH_MAG[3] = sq(q3); - SH_MAG[4] = sq(q2); - SH_MAG[5] = sq(q1); - SH_MAG[6] = sq(q0); - SH_MAG[7] = 2*magN*q0; - SH_MAG[8] = 2*magE*q3; - - for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; - H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - H_MAG[1] = SH_MAG[0]; - H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; - H_MAG[3] = SH_MAG[2]; - H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; - H_MAG[17] = 2*q0*q3 + 2*q1*q2; - H_MAG[18] = 2*q1*q3 - 2*q0*q2; - H_MAG[19] = 1.0f; - - // Calculate Kalman gain - float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); - if (temp >= R_MAG) { - SK_MX[0] = 1.0f / temp; - } else { - // the calculation is badly conditioned, so we cannot perform fusion on this step - // we increase the state variances and try again next time - P[19][19] += 0.1f*R_MAG; - obsIndex = 1; - return; - } - SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; - SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; - SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - SK_MX[4] = 2*q0*q2 - 2*q1*q3; - SK_MX[5] = 2*q0*q3 + 2*q1*q2; - Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]); - Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[5] - P[1][18]*SK_MX[4]); - Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[5] - P[2][18]*SK_MX[4]); - Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[5] - P[3][18]*SK_MX[4]); - Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]); - Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]); - Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]); - Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]); - Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]); - Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]); - Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]); - Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]); - Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]); - // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate - Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]); - Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); - Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); - Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); - Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); - Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); - Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); - Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); - Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); - Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); - varInnovMag[0] = 1.0f/SK_MX[0]; - innovMag[0] = MagPred[0] - magData.x; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - fuseMagData = false; - } - else if (obsIndex == 1) // we are now fusing the Y measurement - { - // Calculate observation jacobians - for (unsigned int i = 0; i < n_states; i++) H_MAG[i] = 0; - H_MAG[0] = SH_MAG[2]; - H_MAG[1] = SH_MAG[1]; - H_MAG[2] = SH_MAG[0]; - H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; - H_MAG[16] = 2*q1*q2 - 2*q0*q3; - H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; - H_MAG[18] = 2*q0*q1 + 2*q2*q3; - H_MAG[20] = 1; - - // Calculate Kalman gain - float temp = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); - if (temp >= R_MAG) { - SK_MY[0] = 1.0f / temp; - } else { - // the calculation is badly conditioned, so we cannot perform fusion on this step - // we increase the state variances and try again next time - P[20][20] += 0.1f*R_MAG; - obsIndex = 2; - return; - } - SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; - SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - SK_MY[3] = 2*q0*q3 - 2*q1*q2; - SK_MY[4] = 2*q0*q1 + 2*q2*q3; - Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]); - Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]); - Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]); - Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]); - Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]); - Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]); - Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]); - Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]); - Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]); - Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]); - Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]); - Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]); - Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); - // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate - Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); - Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); - Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); - Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); - Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); - Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); - Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); - Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); - Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); - Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); - varInnovMag[1] = 1.0f/SK_MY[0]; - innovMag[1] = MagPred[1] - magData.y; - } - else if (obsIndex == 2) // we are now fusing the Z measurement - { - // Calculate observation jacobians - for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; - H_MAG[0] = SH_MAG[1]; - H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; - H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - H_MAG[3] = SH_MAG[0]; - H_MAG[16] = 2*q0*q2 + 2*q1*q3; - H_MAG[17] = 2*q2*q3 - 2*q0*q1; - H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; - H_MAG[21] = 1; - - // Calculate Kalman gain - float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); - if (temp >= R_MAG) { - SK_MZ[0] = 1.0f / temp; - } else { - // the calculation is badly conditioned, so we cannot perform fusion on this step - // we increase the state variances and try again next time - P[21][21] += 0.1f*R_MAG; - obsIndex = 3; - return; - } - SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; - SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; - SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - SK_MZ[4] = 2*q0*q1 - 2*q2*q3; - SK_MZ[5] = 2*q0*q2 + 2*q1*q3; - Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[5] - P[0][17]*SK_MZ[4]); - Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[5] - P[1][17]*SK_MZ[4]); - Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[5] - P[2][17]*SK_MZ[4]); - Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[5] - P[3][17]*SK_MZ[4]); - Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[5] - P[4][17]*SK_MZ[4]); - Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[5] - P[5][17]*SK_MZ[4]); - Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[5] - P[6][17]*SK_MZ[4]); - Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[5] - P[7][17]*SK_MZ[4]); - Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[5] - P[8][17]*SK_MZ[4]); - Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[5] - P[9][17]*SK_MZ[4]); - Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[5] - P[10][17]*SK_MZ[4]); - Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[5] - P[11][17]*SK_MZ[4]); - Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]); - // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate - Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]); - Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]); - Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]); - Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]); - Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]); - Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]); - Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]); - Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]); - Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]); - Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]); - varInnovMag[2] = 1.0f/SK_MZ[0]; - innovMag[2] = MagPred[2] - magData.z; - - } - - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) - { - // correct the state vector - for (uint8_t j= 0; j < indexLimit; j++) - { - states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; - } - // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12) - { - for (uint8_t j= 0; j<=3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations - for (uint8_t i = 0; i < indexLimit; i++) - { - for (uint8_t j = 0; j <= 3; j++) - { - KH[i][j] = Kfusion[i] * H_MAG[j]; - } - for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f; - if (!onGround) - { - for (uint8_t j = 16; j <= 21; j++) - { - KH[i][j] = Kfusion[i] * H_MAG[j]; - } - } - else - { - for (uint8_t j = 16; j <= 21; j++) - { - KH[i][j] = 0.0f; - } - } - } - for (uint8_t i = 0; i < indexLimit; i++) - { - for (uint8_t j = 0; j < indexLimit; j++) - { - KHP[i][j] = 0.0f; - for (uint8_t k = 0; k <= 3; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - if (!onGround) - { - for (uint8_t k = 16; k<=21; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - } - } - } - } - for (uint8_t i = 0; i < indexLimit; i++) - { - for (uint8_t j = 0; j < indexLimit; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - obsIndex = obsIndex + 1; - - ForceSymmetry(); - ConstrainVariances(); -} - -void AttPosEKF::FuseAirspeed() -{ - float vn; - float ve; - float vd; - float vwn; - float vwe; - float R_TAS = sq(airspeedMeasurementSigma); - float SH_TAS[3]; - float SK_TAS; - float VtasPred; - - // Copy required states to local variable names - vn = statesAtVtasMeasTime[4]; - ve = statesAtVtasMeasTime[5]; - vd = statesAtVtasMeasTime[6]; - vwn = statesAtVtasMeasTime[14]; - vwe = statesAtVtasMeasTime[15]; - - // Need to check that it is flying before fusing airspeed data - // Calculate the predicted airspeed - VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); - // Perform fusion of True Airspeed measurement - if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) - { - // Calculate observation jacobians - SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); - SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; - SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; - - float H_TAS[n_states]; - for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f; - H_TAS[4] = SH_TAS[2]; - H_TAS[5] = SH_TAS[1]; - H_TAS[6] = vd*SH_TAS[0]; - H_TAS[14] = -SH_TAS[2]; - H_TAS[15] = -SH_TAS[1]; - - // Calculate Kalman gains - float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); - if (temp >= R_TAS) { - SK_TAS = 1.0f / temp; - } else { - // the calculation is badly conditioned, so we cannot perform fusion on this step - // we increase the wind state variances and try again next time - P[14][14] += 0.05f*R_TAS; - P[15][15] += 0.05f*R_TAS; - return; - } - Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); - Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); - Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][14]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][15]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); - Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][14]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][15]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); - Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][14]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][15]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); - Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][14]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][15]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); - Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][14]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][15]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); - Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][14]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][15]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); - Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][14]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][15]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); - Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][14]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][15]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); - Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][14]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][15]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); - Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][14]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][15]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); - Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); - // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate - Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); - Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); - Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); - Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); - Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); - Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); - Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); - Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); - Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); - Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]); - varInnovVtas = 1.0f/SK_TAS; - - // Calculate the measurement innovation - innovVtas = VtasPred - VtasMeas; - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovVtas*innovVtas*SK_TAS) < 25.0) - { - // correct the state vector - for (uint8_t j=0; j <= 22; j++) - { - states[j] = states[j] - Kfusion[j] * innovVtas; - } - // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12f) - { - for (uint8_t j= 0; j <= 3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in H to reduce the - // number of operations - for (uint8_t i = 0; i <= 22; i++) - { - for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0; - for (uint8_t j = 4; j <= 6; j++) - { - KH[i][j] = Kfusion[i] * H_TAS[j]; - } - for (uint8_t j = 7; j <= 13; j++) KH[i][j] = 0.0; - for (uint8_t j = 14; j <= 15; j++) - { - KH[i][j] = Kfusion[i] * H_TAS[j]; - } - for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0; - } - for (uint8_t i = 0; i <= 22; i++) - { - for (uint8_t j = 0; j <= 22; j++) - { - KHP[i][j] = 0.0; - for (uint8_t k = 4; k <= 6; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - for (uint8_t k = 14; k <= 15; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - } - } - for (uint8_t i = 0; i <= 22; i++) - { - for (uint8_t j = 0; j <= 22; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - } - - ForceSymmetry(); - ConstrainVariances(); -} - -void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) -{ - uint8_t row; - uint8_t col; - for (row=first; row<=last; row++) - { - for (col=0; col 0.87f) - { - // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset - // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations - SH_RNG[0] = SH_RNG[4]*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); - SH_RNG[1] = pd - ptd; - SH_RNG[2] = 1/sq(SH_RNG[0]); - SH_RNG[3] = 1/SH_RNG[0]; - for (uint8_t i = 0; i < n_states; i++) { - H_RNG[i] = 0.0f; - Kfusion[i] = 0.0f; - } - H_RNG[22] = -SH_RNG[3]; - SK_RNG[0] = 1/(R_RNG + SH_RNG[3]*(P[9][9]*SH_RNG[3] - P[22][9]*SH_RNG[3] + P[0][9]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][9]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][9]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][9]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[3]*(P[9][22]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[0][22]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][22]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][22]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][22]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4])*(P[9][0]*SH_RNG[3] - P[22][0]*SH_RNG[3] + P[0][0]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][0]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][0]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][0]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4])*(P[9][1]*SH_RNG[3] - P[22][1]*SH_RNG[3] + P[0][1]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][1]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][1]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][1]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4])*(P[9][2]*SH_RNG[3] - P[22][2]*SH_RNG[3] + P[0][2]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][2]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][2]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][2]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])*(P[9][3]*SH_RNG[3] - P[22][3]*SH_RNG[3] + P[0][3]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][3]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][3]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][3]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4]))); - SK_RNG[1] = 2*q1 - 2*q3*SH_RNG[4]; - SK_RNG[2] = 2*q0 - 2*q2*SH_RNG[4]; - SK_RNG[3] = 2*q3 + 2*q1*SH_RNG[4]; - SK_RNG[4] = 2*q2 + 2*q0*SH_RNG[4]; - SK_RNG[5] = SH_RNG[2]; - Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); - - // Calculate the measurement innovation - rngPred = (ptd - pd)/cosRngTilt; - innovRng = rngPred - rngMea; - //printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd); - - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovRng*innovRng*SK_RNG[0]) < 25) - { - // correct the state vector - states[22] = states[22] - Kfusion[22] * innovRng; - - // correct the covariance P = (I - K*H)*P - P[22][22] = P[22][22] - Kfusion[22] * H_RNG[22] * P[22][22]; - P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); - } - } - -} - -void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) -{ - uint8_t row; - uint8_t col; - for (col=first; col<=last; col++) - { - for (row=0; row < n_states; row++) - { - covMat[row][col] = 0.0; - } - } -} - -float AttPosEKF::sq(float valIn) -{ - return valIn*valIn; -} - -// Store states in a history array along with time stamp -void AttPosEKF::StoreStates(uint64_t timestamp_ms) -{ - for (unsigned i=0; i statetimeStamp[storeIndex]) { - timeDelta = msec - statetimeStamp[storeIndex]; - } else { - timeDelta = statetimeStamp[storeIndex] - msec; - } - - if (timeDelta < bestTimeDelta) - { - bestStoreIndex = storeIndex; - bestTimeDelta = timeDelta; - } - } - if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - { - for (unsigned i=0; i < n_states; i++) { - if (isfinite(storedStates[i][bestStoreIndex])) { - statesForFusion[i] = storedStates[i][bestStoreIndex]; - } else if (isfinite(states[i])) { - statesForFusion[i] = states[i]; - } else { - // There is not much we can do here, except reporting the error we just - // found. - ret++; - } - } - } - else // otherwise output current state - { - for (unsigned i = 0; i < n_states; i++) { - if (isfinite(states[i])) { - statesForFusion[i] = states[i]; - } else { - ret++; - } - } - } - - return ret; -} - -void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) -{ - // Calculate the nav to body cosine matrix - float q00 = sq(quat[0]); - float q11 = sq(quat[1]); - float q22 = sq(quat[2]); - float q33 = sq(quat[3]); - float q01 = quat[0]*quat[1]; - float q02 = quat[0]*quat[2]; - float q03 = quat[0]*quat[3]; - float q12 = quat[1]*quat[2]; - float q13 = quat[1]*quat[3]; - float q23 = quat[2]*quat[3]; - - Tnb.x.x = q00 + q11 - q22 - q33; - Tnb.y.y = q00 - q11 + q22 - q33; - Tnb.z.z = q00 - q11 - q22 + q33; - Tnb.y.x = 2*(q12 - q03); - Tnb.z.x = 2*(q13 + q02); - Tnb.x.y = 2*(q12 + q03); - Tnb.z.y = 2*(q23 - q01); - Tnb.x.z = 2*(q13 - q02); - Tnb.y.z = 2*(q23 + q01); -} - -void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) -{ - // Calculate the body to nav cosine matrix - float q00 = sq(quat[0]); - float q11 = sq(quat[1]); - float q22 = sq(quat[2]); - float q33 = sq(quat[3]); - float q01 = quat[0]*quat[1]; - float q02 = quat[0]*quat[2]; - float q03 = quat[0]*quat[3]; - float q12 = quat[1]*quat[2]; - float q13 = quat[1]*quat[3]; - float q23 = quat[2]*quat[3]; - - Tbn.x.x = q00 + q11 - q22 - q33; - Tbn.y.y = q00 - q11 + q22 - q33; - Tbn.z.z = q00 - q11 - q22 + q33; - Tbn.x.y = 2*(q12 - q03); - Tbn.x.z = 2*(q13 + q02); - Tbn.y.x = 2*(q12 + q03); - Tbn.y.z = 2*(q23 - q01); - Tbn.z.x = 2*(q13 - q02); - Tbn.z.y = 2*(q23 + q01); -} - -void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3]) -{ - float u1 = cos(0.5f*eul[0]); - float u2 = cos(0.5f*eul[1]); - float u3 = cos(0.5f*eul[2]); - float u4 = sin(0.5f*eul[0]); - float u5 = sin(0.5f*eul[1]); - float u6 = sin(0.5f*eul[2]); - quat[0] = u1*u2*u3+u4*u5*u6; - quat[1] = u4*u2*u3-u1*u5*u6; - quat[2] = u1*u5*u3+u4*u2*u6; - quat[3] = u1*u2*u6-u4*u5*u3; -} - -void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) -{ - y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3])); - y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2])); - y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); -} - -void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) -{ - velNED[0] = gpsGndSpd*cosf(gpsCourse); - velNED[1] = gpsGndSpd*sinf(gpsCourse); - velNED[2] = gpsVelD; -} - -void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef) -{ - posNED[0] = earthRadius * (lat - latRef); - posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); - posNED[2] = -(hgt - hgtRef); -} - -void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) -{ - lat = latRef + posNED[0] * earthRadiusInv; - lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); - hgt = hgtRef - posNED[2]; -} - -void AttPosEKF::OnGroundCheck() -{ - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); - if (staticMode) { - staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); - } -} - -void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) -{ - //Define Earth rotation vector in the NED navigation frame - omega.x = earthRate*cosf(latitude); - omega.y = 0.0f; - omega.z = -earthRate*sinf(latitude); -} - -void AttPosEKF::CovarianceInit() -{ - // Calculate the initial covariance matrix P - P[0][0] = 0.25f * sq(1.0f*deg2rad); - P[1][1] = 0.25f * sq(1.0f*deg2rad); - P[2][2] = 0.25f * sq(1.0f*deg2rad); - P[3][3] = 0.25f * sq(10.0f*deg2rad); - P[4][4] = sq(0.7f); - P[5][5] = P[4][4]; - P[6][6] = sq(0.7f); - P[7][7] = sq(15.0f); - P[8][8] = P[7][7]; - P[9][9] = sq(5.0f); - P[10][10] = sq(0.1f*deg2rad*dtIMU); - P[11][11] = P[10][10]; - P[12][12] = P[10][10]; - P[13][13] = sq(0.2f*dtIMU); - P[14][14] = sq(8.0f); - P[15][14] = P[14][14]; - P[16][16] = sq(0.02f); - P[17][17] = P[16][16]; - P[18][18] = P[16][16]; - P[19][19] = sq(0.02f); - P[20][20] = P[19][19]; - P[21][21] = P[19][19]; - P[22][22] = sq(0.5f); -} - -float AttPosEKF::ConstrainFloat(float val, float min, float max) -{ - float ret; - if (val > max) { - ret = max; - ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val); - } else if (val < min) { - ret = min; - ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val); - } else { - ret = val; - } - - if (!isfinite(val)) { - ekf_debug("constrain: non-finite!"); - } - - return ret; -} - -void AttPosEKF::ConstrainVariances() -{ - if (!numericalProtection) { - return; - } - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13: Delta Velocity bias - m/s (Z) - // 14-15: Wind Vector - m/sec (North,East) - // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) - // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - // 22: Terrain offset - m - - // Constrain quaternion variances - for (unsigned i = 0; i <= 3; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - - // Constrain velocity variances - for (unsigned i = 4; i <= 6; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); - } - - // Constrain position variances - for (unsigned i = 7; i <= 9; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); - } - - // Constrain delta angle bias variances - for (unsigned i = 10; i <= 12; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU)); - } - - // Constrain delta velocity bias variance - P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU)); - - // Wind velocity variances - for (unsigned i = 14; i <= 15; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); - } - - // Earth magnetic field variances - for (unsigned i = 16; i <= 18; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - - // Body magnetic field variances - for (unsigned i = 19; i <= 21; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - - // Constrain terrain offset variance - P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); -} - -void AttPosEKF::ConstrainStates() -{ - if (!numericalProtection) { - return; - } - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13: Delta Velocity bias - m/s (Z) - // 14-15: Wind Vector - m/sec (North,East) - // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) - // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - // 22: Terrain offset - m - - // Constrain quaternion - for (unsigned i = 0; i <= 3; i++) { - states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); - } - - // Constrain velocities to what GPS can do for us - for (unsigned i = 4; i <= 6; i++) { - states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); - } - - // Constrain position to a reasonable vehicle range (in meters) - for (unsigned i = 7; i <= 8; i++) { - states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); - } - - // Constrain altitude - states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); - - // Angle bias limit - set to 8 degrees / sec - for (unsigned i = 10; i <= 12; i++) { - states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); - } - - // Constrain delta velocity bias - states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); - - // Wind velocity limits - assume 120 m/s max velocity - for (unsigned i = 14; i <= 15; i++) { - states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); - } - - // Earth magnetic field limits (in Gauss) - for (unsigned i = 16; i <= 18; i++) { - states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); - } - - // Body magnetic field variances (in Gauss). - // the max offset should be in this range. - for (unsigned i = 19; i <= 21; i++) { - states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); - } - - // Constrain terrain offset - states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f); - -} - -void AttPosEKF::ForceSymmetry() -{ - if (!numericalProtection) { - return; - } - - // Force symmetry on the covariance matrix to prevent ill-conditioning - // of the matrix which would cause the filter to blow-up - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (P[i][j] + P[j][i]); - P[j][i] = P[i][j]; - } - } -} - -bool AttPosEKF::FilterHealthy() -{ - if (!statesInitialised) { - return false; - } - - // XXX Check state vector for NaNs and ill-conditioning - - // Check if any of the major inputs timed out - if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { - return false; - } - - // Nothing fired, return ok. - return true; -} - -/** - * Reset the filter position states - * - * This resets the position to the last GPS measurement - * or to zero in case of static position. - */ -void AttPosEKF::ResetPosition(void) -{ - if (staticMode) { - states[7] = 0; - states[8] = 0; - } else if (GPSstatus >= GPS_FIX_3D) { - - // reset the states from the GPS measurements - states[7] = posNE[0]; - states[8] = posNE[1]; - } -} - -/** - * Reset the height state. - * - * This resets the height state with the last altitude measurements - */ -void AttPosEKF::ResetHeight(void) -{ - // write to the state vector - states[9] = -hgtMea; -} - -/** - * Reset the velocity state. - */ -void AttPosEKF::ResetVelocity(void) -{ - if (staticMode) { - states[4] = 0.0f; - states[5] = 0.0f; - states[6] = 0.0f; - } else if (GPSstatus >= GPS_FIX_3D) { - - states[4] = velNED[0]; // north velocity from last reading - states[5] = velNED[1]; // east velocity from last reading - states[6] = velNED[2]; // down velocity from last reading - } -} - - -void AttPosEKF::FillErrorReport(struct ekf_status_report *err) -{ - for (unsigned i = 0; i < n_states; i++) - { - err->states[i] = states[i]; - } - - err->velHealth = current_ekf_state.velHealth; - err->posHealth = current_ekf_state.posHealth; - err->hgtHealth = current_ekf_state.hgtHealth; - err->velTimeout = current_ekf_state.velTimeout; - err->posTimeout = current_ekf_state.posTimeout; - err->hgtTimeout = current_ekf_state.hgtTimeout; -} - -bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { - bool err = false; - - // check all integrators - if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { - err_report->angNaN = true; - ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); - err = true; - goto out; - } // delta angles - - if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { - err_report->angNaN = true; - ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); - err = true; - goto out; - } // delta angles - - if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { - err_report->summedDelVelNaN = true; - ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); - err = true; - goto out; - } // delta velocities - - // check all states and covariance matrices - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - if (!isfinite(KH[i][j])) { - - err_report->KHNaN = true; - err = true; - ekf_debug("KH NaN"); - goto out; - } // intermediate result used for covariance updates - - if (!isfinite(KHP[i][j])) { - - err_report->KHPNaN = true; - err = true; - ekf_debug("KHP NaN"); - goto out; - } // intermediate result used for covariance updates - - if (!isfinite(P[i][j])) { - - err_report->covarianceNaN = true; - err = true; - ekf_debug("P NaN"); - } // covariance matrix - } - - if (!isfinite(Kfusion[i])) { - - err_report->kalmanGainsNaN = true; - ekf_debug("Kfusion NaN"); - err = true; - goto out; - } // Kalman gains - - if (!isfinite(states[i])) { - - err_report->statesNaN = true; - ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); - err = true; - goto out; - } // state matrix - } - -out: - if (err) { - FillErrorReport(err_report); - } - - return err; - -} - -/** - * Check the filter inputs and bound its operational state - * - * This check will reset the filter states if required - * due to a failure of consistency or timeout checks. - * it should be run after the measurement data has been - * updated, but before any of the fusion steps are - * executed. - */ -int AttPosEKF::CheckAndBound() -{ - - // Store the old filter state - bool currStaticMode = staticMode; - - // Reset the filter if the states went NaN - if (StatesNaN(&last_ekf_error)) { - ekf_debug("re-initializing dynamic"); - - InitializeDynamic(velNED, magDeclination); - - return 1; - } - - // Reset the filter if the IMU data is too old - if (dtIMU > 0.3f) { - FillErrorReport(&last_ekf_error); - ResetVelocity(); - ResetPosition(); - ResetHeight(); - ResetStoredStates(); - - // that's all we can do here, return - return 2; - } - - // Check if we're on ground - this also sets static mode. - OnGroundCheck(); - - // Check if we switched between states - if (currStaticMode != staticMode) { - FillErrorReport(&last_ekf_error); - ResetVelocity(); - ResetPosition(); - ResetHeight(); - ResetStoredStates(); - - return 3; - } - - // Reset the filter if gyro offsets are excessive - if (fabs(states[10]) > 1.0f || fabsf(states[11]) > 1.0f || fabsf(states[12]) > 1.0f) { - - InitializeDynamic(velNED, magDeclination); - - // that's all we can do here, return - return 4; - } - - return 0; -} - -void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) -{ - float initialRoll, initialPitch; - float cosRoll, sinRoll, cosPitch, sinPitch; - float magX, magY; - float initialHdg, cosHeading, sinHeading; - - initialRoll = atan2f(-ay, -az); - initialPitch = atan2f(ax, -az); - - cosRoll = cosf(initialRoll); - sinRoll = sinf(initialRoll); - cosPitch = cosf(initialPitch); - sinPitch = sinf(initialPitch); - - magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; - - magY = my * cosRoll - mz * sinRoll; - - initialHdg = atan2f(-magY, magX); - /* true heading is the mag heading minus declination */ - initialHdg += declination; - - cosRoll = cosf(initialRoll * 0.5f); - sinRoll = sinf(initialRoll * 0.5f); - - cosPitch = cosf(initialPitch * 0.5f); - sinPitch = sinf(initialPitch * 0.5f); - - cosHeading = cosf(initialHdg * 0.5f); - sinHeading = sinf(initialHdg * 0.5f); - - initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; - - /* normalize */ - float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]); - - initQuat[0] /= norm; - initQuat[1] /= norm; - initQuat[2] /= norm; - initQuat[3] /= norm; -} - -void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) -{ - - // Fill variables with valid data - velNED[0] = initvelNED[0]; - velNED[1] = initvelNED[1]; - velNED[2] = initvelNED[2]; - magDeclination = declination; - - // Calculate initial filter quaternion states from raw measurements - float initQuat[4]; - Vector3f initMagXYZ; - initMagXYZ = magData - magBias; - AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, declination, initQuat); - - // Calculate initial Tbn matrix and rotate Mag measurements into NED - // to set initial NED magnetic field states - Mat3f DCM; - quat2Tbn(DCM, initQuat); - Vector3f initMagNED; - initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; - initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; - initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; - - magstate.q0 = initQuat[0]; - magstate.q1 = initQuat[1]; - magstate.q2 = initQuat[2]; - magstate.q3 = initQuat[3]; - magstate.magN = initMagNED.x; - magstate.magE = initMagNED.y; - magstate.magD = initMagNED.z; - magstate.magXbias = magBias.x; - magstate.magYbias = magBias.y; - magstate.magZbias = magBias.z; - magstate.R_MAG = sq(magMeasurementSigma); - magstate.DCM = DCM; - - // write to state vector - for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions - for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities - for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel - states[16] = initMagNED.x; // Magnetic Field North - states[17] = initMagNED.y; // Magnetic Field East - states[18] = initMagNED.z; // Magnetic Field Down - states[19] = magBias.x; // Magnetic Field Bias X - states[20] = magBias.y; // Magnetic Field Bias Y - states[21] = magBias.z; // Magnetic Field Bias Z - states[22] = 0.0f; // terrain height - - ResetVelocity(); - ResetPosition(); - ResetHeight(); - - statesInitialised = true; - - // initialise the covariance matrix - CovarianceInit(); - - //Define Earth rotation vector in the NED navigation frame - calcEarthRateNED(earthRateNED, latRef); - -} - -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) -{ - // store initial lat,long and height - latRef = referenceLat; - lonRef = referenceLon; - hgtRef = referenceHgt; - refSet = true; - - // we are at reference altitude, so measurement must be zero - hgtMea = 0.0f; - posNE[0] = 0.0f; - posNE[1] = 0.0f; - - // the baro offset must be this difference now - baroHgtOffset = baroHgt - referenceHgt; - - InitializeDynamic(initvelNED, declination); -} - -void AttPosEKF::ZeroVariables() -{ - - // Initialize on-init initialized variables - fusionModeGPS = 0; - covSkipCount = 0; - statesInitialised = false; - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - onGround = true; - staticMode = true; - useAirspeed = true; - useCompass = true; - useRangeFinder = true; - numericalProtection = true; - refSet = false; - storeIndex = 0; - gpsHgt = 0.0f; - baroHgt = 0.0f; - GPSstatus = 0; - VtasMeas = 0.0f; - magDeclination = 0.0f; - - // Do the data structure init - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - KH[i][j] = 0.0f; // intermediate result used for covariance updates - KHP[i][j] = 0.0f; // intermediate result used for covariance updates - P[i][j] = 0.0f; // covariance matrix - } - - Kfusion[i] = 0.0f; // Kalman gains - states[i] = 0.0f; // state matrix - } - - correctedDelAng.zero(); - summedDelAng.zero(); - summedDelVel.zero(); - - dAngIMU.zero(); - dVelIMU.zero(); - - for (unsigned i = 0; i < data_buffer_size; i++) { - - for (unsigned j = 0; j < n_states; j++) { - storedStates[j][i] = 0.0f; - } - - statetimeStamp[i] = 0; - } - - memset(&magstate, 0, sizeof(magstate)); - magstate.q0 = 1.0f; - magstate.DCM.identity(); - - memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); - -} - -void AttPosEKF::GetFilterState(struct ekf_status_report *state) -{ - - // Copy states - for (unsigned i = 0; i < n_states; i++) { - current_ekf_state.states[i] = states[i]; - } - - memcpy(state, ¤t_ekf_state, sizeof(*state)); -} - -void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) -{ - memcpy(last_error, &last_ekf_error, sizeof(*last_error)); -} diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h deleted file mode 100644 index 401462923..000000000 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ /dev/null @@ -1,356 +0,0 @@ -#include -#include - -#pragma once - -#define GRAVITY_MSS 9.80665f -#define deg2rad 0.017453292f -#define rad2deg 57.295780f -#define pi 3.141592657f -#define earthRate 0.000072921f -#define earthRadius 6378145.0f -#define earthRadiusInv 1.5678540e-7f - -class Vector3f -{ -private: -public: - float x; - float y; - float z; - - float length(void) const; - void zero(void); -}; - -class Mat3f -{ -private: -public: - Vector3f x; - Vector3f y; - Vector3f z; - - Mat3f(); - - void identity(); - Mat3f transpose(void) const; -}; - -Vector3f operator*(float sclIn1, Vector3f vecIn1); -Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); -Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); -Vector3f operator*( Mat3f matIn, Vector3f vecIn); -Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); -Vector3f operator*(Vector3f vecIn1, float sclIn1); - -void swap_var(float &d1, float &d2); - -const unsigned int n_states = 23; -const unsigned int data_buffer_size = 50; - -enum GPS_FIX { - GPS_FIX_NOFIX = 0, - GPS_FIX_2D = 2, - GPS_FIX_3D = 3 -}; - -struct ekf_status_report { - bool velHealth; - bool posHealth; - bool hgtHealth; - bool velTimeout; - bool posTimeout; - bool hgtTimeout; - uint32_t velFailTime; - uint32_t posFailTime; - uint32_t hgtFailTime; - float states[n_states]; - bool angNaN; - bool summedDelVelNaN; - bool KHNaN; - bool KHPNaN; - bool PNaN; - bool covarianceNaN; - bool kalmanGainsNaN; - bool statesNaN; -}; - -class AttPosEKF { - -public: - - AttPosEKF(); - ~AttPosEKF(); - - - - /* ############################################## - * - * M A I N F I L T E R P A R A M E T E R S - * - * ########################################### */ - - /* - * parameters are defined here and initialised in - * the InitialiseParameters() (which is just 20 lines down) - */ - - float covTimeStepMax; // maximum time allowed between covariance predictions - float covDelAngMax; // maximum delta angle between covariance predictions - float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. - - float yawVarScale; - float windVelSigma; - float dAngBiasSigma; - float dVelBiasSigma; - float magEarthSigma; - float magBodySigma; - float gndHgtSigma; - - float vneSigma; - float vdSigma; - float posNeSigma; - float posDSigma; - float magMeasurementSigma; - float airspeedMeasurementSigma; - - float gyroProcessNoise; - float accelProcessNoise; - - float EAS2TAS; // ratio f true to equivalent airspeed - - void InitialiseParameters() - { - covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions - covDelAngMax = 0.02f; // maximum delta angle between covariance predictions - rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. - EAS2TAS = 1.0f; - - yawVarScale = 1.0f; - windVelSigma = 0.1f; - dAngBiasSigma = 5.0e-7f; - dVelBiasSigma = 1e-4f; - magEarthSigma = 3.0e-4f; - magBodySigma = 3.0e-4f; - gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma - - vneSigma = 0.2f; - vdSigma = 0.3f; - posNeSigma = 2.0f; - posDSigma = 2.0f; - - magMeasurementSigma = 0.05; - airspeedMeasurementSigma = 1.4f; - gyroProcessNoise = 1.4544411e-2f; - accelProcessNoise = 0.5f; - } - - struct { - unsigned obsIndex; - float MagPred[3]; - float SH_MAG[9]; - float q0; - float q1; - float q2; - float q3; - float magN; - float magE; - float magD; - float magXbias; - float magYbias; - float magZbias; - float R_MAG; - Mat3f DCM; - } magstate; - - - // Global variables - float KH[n_states][n_states]; // intermediate result used for covariance updates - float KHP[n_states][n_states]; // intermediate result used for covariance updates - float P[n_states][n_states]; // covariance matrix - float Kfusion[n_states]; // Kalman gains - float states[n_states]; // state matrix - float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps - uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored - - float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements - float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements - float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement - float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time - float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time - float statesAtRngTime[n_states]; // filter states at the effective measurement time - - Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) - Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) - Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) - Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) - float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) - Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) - Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) - - Mat3f Tbn; // transformation matrix from body to NED coordinates - Mat3f Tnb; // transformation amtrix from NED to body coordinates - - Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) - Vector3f dVelIMU; - Vector3f dAngIMU; - float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) - uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity - float innovVelPos[6]; // innovation output - float varInnovVelPos[6]; // innovation variance output - - float velNED[3]; // North, East, Down velocity obs (m/s) - float posNE[2]; // North, East position obs (m) - float hgtMea; // measured height (m) - float baroHgtOffset; ///< the baro (weather) offset from normalized altitude - float rngMea; // Ground distance - - float innovMag[3]; // innovation output - float varInnovMag[3]; // innovation variance output - Vector3f magData; // magnetometer flux radings in X,Y,Z body axes - float innovVtas; // innovation output - float innovRng; ///< Range finder innovation - float varInnovVtas; // innovation variance output - float VtasMeas; // true airspeed measurement (m/s) - float magDeclination; ///< magnetic declination - double latRef; // WGS-84 latitude of reference point (rad) - double lonRef; // WGS-84 longitude of reference point (rad) - float hgtRef; // WGS-84 height of reference point (m) - bool refSet; ///< flag to indicate if the reference position has been set - Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes - unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction - - // GPS input data variables - float gpsCourse; - float gpsVelD; - double gpsLat; - double gpsLon; - float gpsHgt; - uint8_t GPSstatus; - - // Baro input - float baroHgt; - - bool statesInitialised; - - bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused - bool fusePosData; // this boolean causes the posNE and velNED obs to be fused - bool fuseHgtData; // this boolean causes the hgtMea obs to be fused - bool fuseMagData; // boolean true when magnetometer data is to be fused - bool fuseVtasData; // boolean true when airspeed data is to be fused - bool fuseRngData; ///< true when range data is fused - - bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) - bool staticMode; ///< boolean true if no position feedback is fused - bool useAirspeed; ///< boolean true if airspeed data is being used - bool useCompass; ///< boolean true if magnetometer data is being used - bool useRangeFinder; ///< true when rangefinder is being used - - struct ekf_status_report current_ekf_state; - struct ekf_status_report last_ekf_error; - - bool numericalProtection; - - unsigned storeIndex; - - -void UpdateStrapdownEquationsNED(); - -void CovariancePrediction(float dt); - -void FuseVelposNED(); - -void FuseMagnetometer(); - -void FuseAirspeed(); - -void FuseRangeFinder(); - -void FuseOpticalFlow(); - -void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); - -void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); - -void quatNorm(float (&quatOut)[4], const float quatIn[4]); - -// store staes along with system time stamp in msces -void StoreStates(uint64_t timestamp_ms); - -/** - * Recall the state vector. - * - * Recalls the vector stored at closest time to the one specified by msec - * - * @return zero on success, integer indicating the number of invalid states on failure. - * Does only copy valid states, if the statesForFusion vector was initialized - * correctly by the caller, the result can be safely used, but is a mixture - * time-wise where valid states were updated and invalid remained at the old - * value. - */ -int RecallStates(float *statesForFusion, uint64_t msec); - -void ResetStoredStates(); - -void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); - -void calcEarthRateNED(Vector3f &omega, float latitude); - -static void eul2quat(float (&quat)[4], const float (&eul)[3]); - -static void quat2eul(float (&eul)[3], const float (&quat)[4]); - -static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); - -static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); - -static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); - -static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); - -static float sq(float valIn); - -void OnGroundCheck(); - -void CovarianceInit(); - -void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); - -float ConstrainFloat(float val, float min, float max); - -void ConstrainVariances(); - -void ConstrainStates(); - -void ForceSymmetry(); - -int CheckAndBound(); - -void ResetPosition(); - -void ResetVelocity(); - -void ZeroVariables(); - -void GetFilterState(struct ekf_status_report *state); - -void GetLastErrorState(struct ekf_status_report *last_error); - -bool StatesNaN(struct ekf_status_report *err_report); -void FillErrorReport(struct ekf_status_report *err); - -void InitializeDynamic(float (&initvelNED)[3], float declination); - -protected: - -bool FilterHealthy(); - -void ResetHeight(void); - -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); - -}; - -uint32_t millis(); - diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp new file mode 100644 index 000000000..ca9db9685 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -0,0 +1,2488 @@ +#include "estimator.h" +#include +#include + +AttPosEKF::AttPosEKF() + + /* NOTE: DO NOT initialize class members here. Use ZeroVariables() + * instead to allow clean in-air re-initialization. + */ +{ + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + ZeroVariables(); + InitialiseParameters(); +} + +AttPosEKF::~AttPosEKF() +{ +} + +void AttPosEKF::UpdateStrapdownEquationsNED() +{ + Vector3f delVelNav; + float q00; + float q11; + float q22; + float q33; + float q01; + float q02; + float q03; + float q12; + float q13; + float q23; + float rotationMag; + float qUpdated[4]; + float quatMag; + double deltaQuat[4]; + const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; + +// Remove sensor bias errors + correctedDelAng.x = dAngIMU.x - states[10]; + correctedDelAng.y = dAngIMU.y - states[11]; + correctedDelAng.z = dAngIMU.z - states[12]; + dVelIMU.x = dVelIMU.x; + dVelIMU.y = dVelIMU.y; + dVelIMU.z = dVelIMU.z - states[13]; + +// Save current measurements + Vector3f prevDelAng = correctedDelAng; + +// Apply corrections for earths rotation rate and coning errors +// * and + operators have been overloaded + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + +// Convert the rotation vector to its equivalent quaternion + rotationMag = correctedDelAng.length(); + if (rotationMag < 1e-12f) + { + deltaQuat[0] = 1.0; + deltaQuat[1] = 0.0; + deltaQuat[2] = 0.0; + deltaQuat[3] = 0.0; + } + else + { + deltaQuat[0] = cos(0.5f*rotationMag); + double rotScaler = (sin(0.5f*rotationMag))/rotationMag; + deltaQuat[1] = correctedDelAng.x*rotScaler; + deltaQuat[2] = correctedDelAng.y*rotScaler; + deltaQuat[3] = correctedDelAng.z*rotScaler; + } + +// Update the quaternions by rotating from the previous attitude through +// the delta angle rotation quaternion + qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; + qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; + qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; + qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; + +// Normalise the quaternions and update the quaternion states + quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); + if (quatMag > 1e-16f) + { + float quatMagInv = 1.0f/quatMag; + states[0] = quatMagInv*qUpdated[0]; + states[1] = quatMagInv*qUpdated[1]; + states[2] = quatMagInv*qUpdated[2]; + states[3] = quatMagInv*qUpdated[3]; + } + +// Calculate the body to nav cosine matrix + q00 = sq(states[0]); + q11 = sq(states[1]); + q22 = sq(states[2]); + q33 = sq(states[3]); + q01 = states[0]*states[1]; + q02 = states[0]*states[2]; + q03 = states[0]*states[3]; + q12 = states[1]*states[2]; + q13 = states[1]*states[3]; + q23 = states[2]*states[3]; + + Tbn.x.x = q00 + q11 - q22 - q33; + Tbn.y.y = q00 - q11 + q22 - q33; + Tbn.z.z = q00 - q11 - q22 + q33; + Tbn.x.y = 2*(q12 - q03); + Tbn.x.z = 2*(q13 + q02); + Tbn.y.x = 2*(q12 + q03); + Tbn.y.z = 2*(q23 - q01); + Tbn.z.x = 2*(q13 - q02); + Tbn.z.y = 2*(q23 + q01); + + Tnb = Tbn.transpose(); + +// transform body delta velocities to delta velocities in the nav frame +// * and + operators have been overloaded + //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; + delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; + delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; + delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; + +// calculate the magnitude of the nav acceleration (required for GPS +// variance estimation) + accNavMag = delVelNav.length()/dtIMU; + +// If calculating position save previous velocity + float lastVelocity[3]; + lastVelocity[0] = states[4]; + lastVelocity[1] = states[5]; + lastVelocity[2] = states[6]; + +// Sum delta velocities to get velocity + states[4] = states[4] + delVelNav.x; + states[5] = states[5] + delVelNav.y; + states[6] = states[6] + delVelNav.z; + +// If calculating postions, do a trapezoidal integration for position + states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; + states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; + states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; + + // Constrain states (to protect against filter divergence) + ConstrainStates(); +} + +void AttPosEKF::CovariancePrediction(float dt) +{ + // scalars + float daxCov; + float dayCov; + float dazCov; + float dvxCov; + float dvyCov; + float dvzCov; + float dvx; + float dvy; + float dvz; + float dax; + float day; + float daz; + float q0; + float q1; + float q2; + float q3; + float dax_b; + float day_b; + float daz_b; + float dvz_b; + + // arrays + float processNoise[n_states]; + float SF[15]; + float SG[8]; + float SQ[11]; + float SPP[8] = {0}; + float nextP[n_states][n_states]; + + // calculate covariance prediction process noise + for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + // scale gyro bias noise when on ground to allow for faster bias estimation + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + processNoise[13] = dVelBiasSigma; + for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; + for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; + for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; + processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + + // square all sigmas + for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); + + // set variables used to calculate covariance growth + dvx = summedDelVel.x; + dvy = summedDelVel.y; + dvz = summedDelVel.z; + dax = summedDelAng.x; + day = summedDelAng.y; + daz = summedDelAng.z; + q0 = states[0]; + q1 = states[1]; + q2 = states[2]; + q3 = states[3]; + dax_b = states[10]; + day_b = states[11]; + daz_b = states[12]; + dvz_b = states[13]; + gyroProcessNoise = ConstrainFloat(gyroProcessNoise, 1e-3f, 5e-2f); + daxCov = sq(dt*gyroProcessNoise); + dayCov = sq(dt*gyroProcessNoise); + dazCov = sq(dt*gyroProcessNoise); + if (onGround) dazCov = dazCov * sq(yawVarScale); + accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f); + dvxCov = sq(dt*accelProcessNoise); + dvyCov = sq(dt*accelProcessNoise); + dvzCov = sq(dt*accelProcessNoise); + + // Predicted covariance calculation + SF[0] = dvz - dvz_b; + SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2; + SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; + SF[3] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; + SF[4] = day/2 - day_b/2; + SF[5] = daz/2 - daz_b/2; + SF[6] = dax/2 - dax_b/2; + SF[7] = dax_b/2 - dax/2; + SF[8] = daz_b/2 - daz/2; + SF[9] = day_b/2 - day/2; + SF[10] = 2*q0*SF[0]; + SF[11] = q1/2; + SF[12] = q2/2; + SF[13] = q3/2; + SF[14] = 2*dvy*q1; + + SG[0] = q0/2; + SG[1] = sq(q3); + SG[2] = sq(q2); + SG[3] = sq(q1); + SG[4] = sq(q0); + SG[5] = 2*q2*q3; + SG[6] = 2*q1*q3; + SG[7] = 2*q1*q2; + + SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); + SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); + SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); + SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4; + SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4; + SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4; + SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4; + SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2; + SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; + SQ[9] = sq(SG[0]); + SQ[10] = sq(q1); + + SPP[0] = SF[10] + SF[14] - 2*dvx*q2; + SPP[1] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; + SPP[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; + SPP[3] = 2*q0*q1 - 2*q2*q3; + SPP[4] = 2*q0*q2 + 2*q1*q3; + SPP[5] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SPP[6] = SF[13]; + SPP[7] = SF[12]; + + nextP[0][0] = P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6] + (daxCov*SQ[10])/4 + SF[7]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[9]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[8]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) + SPP[7]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[6]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6] + SF[6]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[5]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[9]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[6]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) - SPP[7]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - (q0*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]))/2; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6] + SF[4]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[8]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[6]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - SPP[6]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]))/2; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6] + SF[5]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[4]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[7]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SF[11]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[7]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]))/2; + nextP[0][4] = P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6] + SF[3]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[0]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SPP[2]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[4]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][5] = P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6] + SF[2]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[3]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[0]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[3]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][6] = P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6] + SF[2]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[1]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[0]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) - SPP[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][7] = P[0][7] + P[1][7]*SF[7] + P[2][7]*SF[9] + P[3][7]*SF[8] + P[10][7]*SF[11] + P[11][7]*SPP[7] + P[12][7]*SPP[6] + dt*(P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6]); + nextP[0][8] = P[0][8] + P[1][8]*SF[7] + P[2][8]*SF[9] + P[3][8]*SF[8] + P[10][8]*SF[11] + P[11][8]*SPP[7] + P[12][8]*SPP[6] + dt*(P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6]); + nextP[0][9] = P[0][9] + P[1][9]*SF[7] + P[2][9]*SF[9] + P[3][9]*SF[8] + P[10][9]*SF[11] + P[11][9]*SPP[7] + P[12][9]*SPP[6] + dt*(P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6]); + nextP[0][10] = P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]; + nextP[0][11] = P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]; + nextP[0][12] = P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]; + nextP[0][13] = P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]; + nextP[0][14] = P[0][14] + P[1][14]*SF[7] + P[2][14]*SF[9] + P[3][14]*SF[8] + P[10][14]*SF[11] + P[11][14]*SPP[7] + P[12][14]*SPP[6]; + nextP[0][15] = P[0][15] + P[1][15]*SF[7] + P[2][15]*SF[9] + P[3][15]*SF[8] + P[10][15]*SF[11] + P[11][15]*SPP[7] + P[12][15]*SPP[6]; + nextP[0][16] = P[0][16] + P[1][16]*SF[7] + P[2][16]*SF[9] + P[3][16]*SF[8] + P[10][16]*SF[11] + P[11][16]*SPP[7] + P[12][16]*SPP[6]; + nextP[0][17] = P[0][17] + P[1][17]*SF[7] + P[2][17]*SF[9] + P[3][17]*SF[8] + P[10][17]*SF[11] + P[11][17]*SPP[7] + P[12][17]*SPP[6]; + nextP[0][18] = P[0][18] + P[1][18]*SF[7] + P[2][18]*SF[9] + P[3][18]*SF[8] + P[10][18]*SF[11] + P[11][18]*SPP[7] + P[12][18]*SPP[6]; + nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6]; + nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6]; + nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6]; + nextP[0][22] = P[0][22] + P[1][22]*SF[7] + P[2][22]*SF[9] + P[3][22]*SF[8] + P[10][22]*SF[11] + P[11][22]*SPP[7] + P[12][22]*SPP[6]; + nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2); + nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2; + nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2; + nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[4]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SF[11]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[7]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2))/2; + nextP[1][4] = P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[4]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][5] = P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][6] = P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2 + SF[2]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[1]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][7] = P[1][7] + P[0][7]*SF[6] + P[2][7]*SF[5] + P[3][7]*SF[9] + P[11][7]*SPP[6] - P[12][7]*SPP[7] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2); + nextP[1][8] = P[1][8] + P[0][8]*SF[6] + P[2][8]*SF[5] + P[3][8]*SF[9] + P[11][8]*SPP[6] - P[12][8]*SPP[7] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2); + nextP[1][9] = P[1][9] + P[0][9]*SF[6] + P[2][9]*SF[5] + P[3][9]*SF[9] + P[11][9]*SPP[6] - P[12][9]*SPP[7] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2); + nextP[1][10] = P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2; + nextP[1][11] = P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2; + nextP[1][12] = P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2; + nextP[1][13] = P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2; + nextP[1][14] = P[1][14] + P[0][14]*SF[6] + P[2][14]*SF[5] + P[3][14]*SF[9] + P[11][14]*SPP[6] - P[12][14]*SPP[7] - (P[10][14]*q0)/2; + nextP[1][15] = P[1][15] + P[0][15]*SF[6] + P[2][15]*SF[5] + P[3][15]*SF[9] + P[11][15]*SPP[6] - P[12][15]*SPP[7] - (P[10][15]*q0)/2; + nextP[1][16] = P[1][16] + P[0][16]*SF[6] + P[2][16]*SF[5] + P[3][16]*SF[9] + P[11][16]*SPP[6] - P[12][16]*SPP[7] - (P[10][16]*q0)/2; + nextP[1][17] = P[1][17] + P[0][17]*SF[6] + P[2][17]*SF[5] + P[3][17]*SF[9] + P[11][17]*SPP[6] - P[12][17]*SPP[7] - (P[10][17]*q0)/2; + nextP[1][18] = P[1][18] + P[0][18]*SF[6] + P[2][18]*SF[5] + P[3][18]*SF[9] + P[11][18]*SPP[6] - P[12][18]*SPP[7] - (P[10][18]*q0)/2; + nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2; + nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2; + nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2; + nextP[1][22] = P[1][22] + P[0][22]*SF[6] + P[2][22]*SF[5] + P[3][22]*SF[9] + P[11][22]*SPP[6] - P[12][22]*SPP[7] - (P[10][22]*q0)/2; + nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2); + nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2; + nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2; + nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[7]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SF[11]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[7]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2))/2; + nextP[2][4] = P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[4]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2 + SF[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[1]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[4] + P[1][7]*SF[8] + P[3][7]*SF[6] + P[12][7]*SF[11] - P[10][7]*SPP[6] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[4] + P[1][8]*SF[8] + P[3][8]*SF[6] + P[12][8]*SF[11] - P[10][8]*SPP[6] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[4] + P[1][9]*SF[8] + P[3][9]*SF[6] + P[12][9]*SF[11] - P[10][9]*SPP[6] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2); + nextP[2][10] = P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2; + nextP[2][11] = P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2; + nextP[2][12] = P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2; + nextP[2][13] = P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2; + nextP[2][14] = P[2][14] + P[0][14]*SF[4] + P[1][14]*SF[8] + P[3][14]*SF[6] + P[12][14]*SF[11] - P[10][14]*SPP[6] - (P[11][14]*q0)/2; + nextP[2][15] = P[2][15] + P[0][15]*SF[4] + P[1][15]*SF[8] + P[3][15]*SF[6] + P[12][15]*SF[11] - P[10][15]*SPP[6] - (P[11][15]*q0)/2; + nextP[2][16] = P[2][16] + P[0][16]*SF[4] + P[1][16]*SF[8] + P[3][16]*SF[6] + P[12][16]*SF[11] - P[10][16]*SPP[6] - (P[11][16]*q0)/2; + nextP[2][17] = P[2][17] + P[0][17]*SF[4] + P[1][17]*SF[8] + P[3][17]*SF[6] + P[12][17]*SF[11] - P[10][17]*SPP[6] - (P[11][17]*q0)/2; + nextP[2][18] = P[2][18] + P[0][18]*SF[4] + P[1][18]*SF[8] + P[3][18]*SF[6] + P[12][18]*SF[11] - P[10][18]*SPP[6] - (P[11][18]*q0)/2; + nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2; + nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2; + nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2; + nextP[2][22] = P[2][22] + P[0][22]*SF[4] + P[1][22]*SF[8] + P[3][22]*SF[6] + P[12][22]*SF[11] - P[10][22]*SPP[6] - (P[11][22]*q0)/2; + nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2); + nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2; + nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2; + nextP[3][3] = P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[4]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[7]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SF[11]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[7]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2))/2; + nextP[3][4] = P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[4]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2 + SF[2]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[1]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[5] + P[1][7]*SF[4] + P[2][7]*SF[7] - P[11][7]*SF[11] + P[10][7]*SPP[7] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[5] + P[1][8]*SF[4] + P[2][8]*SF[7] - P[11][8]*SF[11] + P[10][8]*SPP[7] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[5] + P[1][9]*SF[4] + P[2][9]*SF[7] - P[11][9]*SF[11] + P[10][9]*SPP[7] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2); + nextP[3][10] = P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2; + nextP[3][11] = P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2; + nextP[3][12] = P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2; + nextP[3][13] = P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2; + nextP[3][14] = P[3][14] + P[0][14]*SF[5] + P[1][14]*SF[4] + P[2][14]*SF[7] - P[11][14]*SF[11] + P[10][14]*SPP[7] - (P[12][14]*q0)/2; + nextP[3][15] = P[3][15] + P[0][15]*SF[5] + P[1][15]*SF[4] + P[2][15]*SF[7] - P[11][15]*SF[11] + P[10][15]*SPP[7] - (P[12][15]*q0)/2; + nextP[3][16] = P[3][16] + P[0][16]*SF[5] + P[1][16]*SF[4] + P[2][16]*SF[7] - P[11][16]*SF[11] + P[10][16]*SPP[7] - (P[12][16]*q0)/2; + nextP[3][17] = P[3][17] + P[0][17]*SF[5] + P[1][17]*SF[4] + P[2][17]*SF[7] - P[11][17]*SF[11] + P[10][17]*SPP[7] - (P[12][17]*q0)/2; + nextP[3][18] = P[3][18] + P[0][18]*SF[5] + P[1][18]*SF[4] + P[2][18]*SF[7] - P[11][18]*SF[11] + P[10][18]*SPP[7] - (P[12][18]*q0)/2; + nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2; + nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2; + nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2; + nextP[3][22] = P[3][22] + P[0][22]*SF[5] + P[1][22]*SF[4] + P[2][22]*SF[7] - P[11][22]*SF[11] + P[10][22]*SPP[7] - (P[12][22]*q0)/2; + nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]); + nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2; + nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2; + nextP[4][3] = P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4] + SF[5]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[4]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[7]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SF[11]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[7]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]))/2; + nextP[4][4] = P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[3]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[0]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SPP[2]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[4]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4] + SF[2]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[3]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[0]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[3]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4] + SF[2]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[1]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[0]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) - SPP[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); + nextP[4][7] = P[4][7] + P[0][7]*SF[3] + P[1][7]*SF[1] + P[2][7]*SPP[0] - P[3][7]*SPP[2] - P[13][7]*SPP[4] + dt*(P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4]); + nextP[4][8] = P[4][8] + P[0][8]*SF[3] + P[1][8]*SF[1] + P[2][8]*SPP[0] - P[3][8]*SPP[2] - P[13][8]*SPP[4] + dt*(P[4][5] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4]); + nextP[4][9] = P[4][9] + P[0][9]*SF[3] + P[1][9]*SF[1] + P[2][9]*SPP[0] - P[3][9]*SPP[2] - P[13][9]*SPP[4] + dt*(P[4][6] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4]); + nextP[4][10] = P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]; + nextP[4][11] = P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]; + nextP[4][12] = P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]; + nextP[4][13] = P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]; + nextP[4][14] = P[4][14] + P[0][14]*SF[3] + P[1][14]*SF[1] + P[2][14]*SPP[0] - P[3][14]*SPP[2] - P[13][14]*SPP[4]; + nextP[4][15] = P[4][15] + P[0][15]*SF[3] + P[1][15]*SF[1] + P[2][15]*SPP[0] - P[3][15]*SPP[2] - P[13][15]*SPP[4]; + nextP[4][16] = P[4][16] + P[0][16]*SF[3] + P[1][16]*SF[1] + P[2][16]*SPP[0] - P[3][16]*SPP[2] - P[13][16]*SPP[4]; + nextP[4][17] = P[4][17] + P[0][17]*SF[3] + P[1][17]*SF[1] + P[2][17]*SPP[0] - P[3][17]*SPP[2] - P[13][17]*SPP[4]; + nextP[4][18] = P[4][18] + P[0][18]*SF[3] + P[1][18]*SF[1] + P[2][18]*SPP[0] - P[3][18]*SPP[2] - P[13][18]*SPP[4]; + nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4]; + nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4]; + nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4]; + nextP[4][22] = P[4][22] + P[0][22]*SF[3] + P[1][22]*SF[1] + P[2][22]*SPP[0] - P[3][22]*SPP[2] - P[13][22]*SPP[4]; + nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]); + nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2; + nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2; + nextP[5][3] = P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3] + SF[5]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[4]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[7]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SF[11]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[7]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]))/2; + nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3] + SF[3]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[0]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SPP[2]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[4]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); + nextP[5][5] = P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[2]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[3]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[0]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[3]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3] + SF[2]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[1]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[0]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) - SPP[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); + nextP[5][7] = P[5][7] + P[0][7]*SF[2] + P[2][7]*SF[1] + P[3][7]*SF[3] - P[1][7]*SPP[0] + P[13][7]*SPP[3] + dt*(P[5][4] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3]); + nextP[5][8] = P[5][8] + P[0][8]*SF[2] + P[2][8]*SF[1] + P[3][8]*SF[3] - P[1][8]*SPP[0] + P[13][8]*SPP[3] + dt*(P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3]); + nextP[5][9] = P[5][9] + P[0][9]*SF[2] + P[2][9]*SF[1] + P[3][9]*SF[3] - P[1][9]*SPP[0] + P[13][9]*SPP[3] + dt*(P[5][6] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3]); + nextP[5][10] = P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]; + nextP[5][11] = P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]; + nextP[5][12] = P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]; + nextP[5][13] = P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]; + nextP[5][14] = P[5][14] + P[0][14]*SF[2] + P[2][14]*SF[1] + P[3][14]*SF[3] - P[1][14]*SPP[0] + P[13][14]*SPP[3]; + nextP[5][15] = P[5][15] + P[0][15]*SF[2] + P[2][15]*SF[1] + P[3][15]*SF[3] - P[1][15]*SPP[0] + P[13][15]*SPP[3]; + nextP[5][16] = P[5][16] + P[0][16]*SF[2] + P[2][16]*SF[1] + P[3][16]*SF[3] - P[1][16]*SPP[0] + P[13][16]*SPP[3]; + nextP[5][17] = P[5][17] + P[0][17]*SF[2] + P[2][17]*SF[1] + P[3][17]*SF[3] - P[1][17]*SPP[0] + P[13][17]*SPP[3]; + nextP[5][18] = P[5][18] + P[0][18]*SF[2] + P[2][18]*SF[1] + P[3][18]*SF[3] - P[1][18]*SPP[0] + P[13][18]*SPP[3]; + nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3]; + nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3]; + nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3]; + nextP[5][22] = P[5][22] + P[0][22]*SF[2] + P[2][22]*SF[1] + P[3][22]*SF[3] - P[1][22]*SPP[0] + P[13][22]*SPP[3]; + nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][3] = P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[5]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[4]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[7]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SF[11]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[3]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[2]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[4]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[2]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[3]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[0]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[3]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][6] = P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) - SPP[5]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]) + SF[2]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[6][7] = P[6][7] + P[1][7]*SF[2] + P[3][7]*SF[1] + P[0][7]*SPP[0] - P[2][7]*SPP[1] - P[13][7]*SPP[5] + dt*(P[6][4] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*SPP[5]); + nextP[6][8] = P[6][8] + P[1][8]*SF[2] + P[3][8]*SF[1] + P[0][8]*SPP[0] - P[2][8]*SPP[1] - P[13][8]*SPP[5] + dt*(P[6][5] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*SPP[5]); + nextP[6][9] = P[6][9] + P[1][9]*SF[2] + P[3][9]*SF[1] + P[0][9]*SPP[0] - P[2][9]*SPP[1] - P[13][9]*SPP[5] + dt*(P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*SPP[5]); + nextP[6][10] = P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*SPP[5]; + nextP[6][11] = P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*SPP[5]; + nextP[6][12] = P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*SPP[5]; + nextP[6][13] = P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]; + nextP[6][14] = P[6][14] + P[1][14]*SF[2] + P[3][14]*SF[1] + P[0][14]*SPP[0] - P[2][14]*SPP[1] - P[13][14]*SPP[5]; + nextP[6][15] = P[6][15] + P[1][15]*SF[2] + P[3][15]*SF[1] + P[0][15]*SPP[0] - P[2][15]*SPP[1] - P[13][15]*SPP[5]; + nextP[6][16] = P[6][16] + P[1][16]*SF[2] + P[3][16]*SF[1] + P[0][16]*SPP[0] - P[2][16]*SPP[1] - P[13][16]*SPP[5]; + nextP[6][17] = P[6][17] + P[1][17]*SF[2] + P[3][17]*SF[1] + P[0][17]*SPP[0] - P[2][17]*SPP[1] - P[13][17]*SPP[5]; + nextP[6][18] = P[6][18] + P[1][18]*SF[2] + P[3][18]*SF[1] + P[0][18]*SPP[0] - P[2][18]*SPP[1] - P[13][18]*SPP[5]; + nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5]; + nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5]; + nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5]; + nextP[6][22] = P[6][22] + P[1][22]*SF[2] + P[3][22]*SF[1] + P[0][22]*SPP[0] - P[2][22]*SPP[1] - P[13][22]*SPP[5]; + nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt); + nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; + nextP[7][2] = P[7][2] + P[4][2]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; + nextP[7][3] = P[7][3] + P[4][3]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][1] + P[4][1]*dt) + SF[7]*(P[7][2] + P[4][2]*dt) - SF[11]*(P[7][11] + P[4][11]*dt) + SPP[7]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; + nextP[7][4] = P[7][4] + P[4][4]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[3]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt) - SPP[4]*(P[7][13] + P[4][13]*dt); + nextP[7][5] = P[7][5] + P[4][5]*dt + SF[2]*(P[7][0] + P[4][0]*dt) + SF[1]*(P[7][2] + P[4][2]*dt) + SF[3]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt) + SPP[3]*(P[7][13] + P[4][13]*dt); + nextP[7][6] = P[7][6] + P[4][6]*dt + SF[2]*(P[7][1] + P[4][1]*dt) + SF[1]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt) - SPP[5]*(P[7][13] + P[4][13]*dt); + nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); + nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); + nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); + nextP[7][10] = P[7][10] + P[4][10]*dt; + nextP[7][11] = P[7][11] + P[4][11]*dt; + nextP[7][12] = P[7][12] + P[4][12]*dt; + nextP[7][13] = P[7][13] + P[4][13]*dt; + nextP[7][14] = P[7][14] + P[4][14]*dt; + nextP[7][15] = P[7][15] + P[4][15]*dt; + nextP[7][16] = P[7][16] + P[4][16]*dt; + nextP[7][17] = P[7][17] + P[4][17]*dt; + nextP[7][18] = P[7][18] + P[4][18]*dt; + nextP[7][19] = P[7][19] + P[4][19]*dt; + nextP[7][20] = P[7][20] + P[4][20]*dt; + nextP[7][21] = P[7][21] + P[4][21]*dt; + nextP[7][22] = P[7][22] + P[4][22]*dt; + nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt); + nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; + nextP[8][2] = P[8][2] + P[5][2]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; + nextP[8][3] = P[8][3] + P[5][3]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][1] + P[5][1]*dt) + SF[7]*(P[8][2] + P[5][2]*dt) - SF[11]*(P[8][11] + P[5][11]*dt) + SPP[7]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; + nextP[8][4] = P[8][4] + P[5][4]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[3]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt) - SPP[4]*(P[8][13] + P[5][13]*dt); + nextP[8][5] = P[8][5] + P[5][5]*dt + SF[2]*(P[8][0] + P[5][0]*dt) + SF[1]*(P[8][2] + P[5][2]*dt) + SF[3]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt) + SPP[3]*(P[8][13] + P[5][13]*dt); + nextP[8][6] = P[8][6] + P[5][6]*dt + SF[2]*(P[8][1] + P[5][1]*dt) + SF[1]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt) - SPP[5]*(P[8][13] + P[5][13]*dt); + nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); + nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); + nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); + nextP[8][10] = P[8][10] + P[5][10]*dt; + nextP[8][11] = P[8][11] + P[5][11]*dt; + nextP[8][12] = P[8][12] + P[5][12]*dt; + nextP[8][13] = P[8][13] + P[5][13]*dt; + nextP[8][14] = P[8][14] + P[5][14]*dt; + nextP[8][15] = P[8][15] + P[5][15]*dt; + nextP[8][16] = P[8][16] + P[5][16]*dt; + nextP[8][17] = P[8][17] + P[5][17]*dt; + nextP[8][18] = P[8][18] + P[5][18]*dt; + nextP[8][19] = P[8][19] + P[5][19]*dt; + nextP[8][20] = P[8][20] + P[5][20]*dt; + nextP[8][21] = P[8][21] + P[5][21]*dt; + nextP[8][22] = P[8][22] + P[5][22]*dt; + nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt); + nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; + nextP[9][2] = P[9][2] + P[6][2]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; + nextP[9][3] = P[9][3] + P[6][3]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][1] + P[6][1]*dt) + SF[7]*(P[9][2] + P[6][2]*dt) - SF[11]*(P[9][11] + P[6][11]*dt) + SPP[7]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; + nextP[9][4] = P[9][4] + P[6][4]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[3]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt) - SPP[4]*(P[9][13] + P[6][13]*dt); + nextP[9][5] = P[9][5] + P[6][5]*dt + SF[2]*(P[9][0] + P[6][0]*dt) + SF[1]*(P[9][2] + P[6][2]*dt) + SF[3]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt) + SPP[3]*(P[9][13] + P[6][13]*dt); + nextP[9][6] = P[9][6] + P[6][6]*dt + SF[2]*(P[9][1] + P[6][1]*dt) + SF[1]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt) - SPP[5]*(P[9][13] + P[6][13]*dt); + nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); + nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); + nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); + nextP[9][10] = P[9][10] + P[6][10]*dt; + nextP[9][11] = P[9][11] + P[6][11]*dt; + nextP[9][12] = P[9][12] + P[6][12]*dt; + nextP[9][13] = P[9][13] + P[6][13]*dt; + nextP[9][14] = P[9][14] + P[6][14]*dt; + nextP[9][15] = P[9][15] + P[6][15]*dt; + nextP[9][16] = P[9][16] + P[6][16]*dt; + nextP[9][17] = P[9][17] + P[6][17]*dt; + nextP[9][18] = P[9][18] + P[6][18]*dt; + nextP[9][19] = P[9][19] + P[6][19]*dt; + nextP[9][20] = P[9][20] + P[6][20]*dt; + nextP[9][21] = P[9][21] + P[6][21]*dt; + nextP[9][22] = P[9][22] + P[6][22]*dt; + nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6]; + nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2; + nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2; + nextP[10][3] = P[10][3] + P[10][0]*SF[5] + P[10][1]*SF[4] + P[10][2]*SF[7] - P[10][11]*SF[11] + P[10][10]*SPP[7] - (P[10][12]*q0)/2; + nextP[10][4] = P[10][4] + P[10][1]*SF[1] + P[10][0]*SF[3] + P[10][2]*SPP[0] - P[10][3]*SPP[2] - P[10][13]*SPP[4]; + nextP[10][5] = P[10][5] + P[10][0]*SF[2] + P[10][2]*SF[1] + P[10][3]*SF[3] - P[10][1]*SPP[0] + P[10][13]*SPP[3]; + nextP[10][6] = P[10][6] + P[10][1]*SF[2] + P[10][3]*SF[1] + P[10][0]*SPP[0] - P[10][2]*SPP[1] - P[10][13]*SPP[5]; + nextP[10][7] = P[10][7] + P[10][4]*dt; + nextP[10][8] = P[10][8] + P[10][5]*dt; + nextP[10][9] = P[10][9] + P[10][6]*dt; + nextP[10][10] = P[10][10]; + nextP[10][11] = P[10][11]; + nextP[10][12] = P[10][12]; + nextP[10][13] = P[10][13]; + nextP[10][14] = P[10][14]; + nextP[10][15] = P[10][15]; + nextP[10][16] = P[10][16]; + nextP[10][17] = P[10][17]; + nextP[10][18] = P[10][18]; + nextP[10][19] = P[10][19]; + nextP[10][20] = P[10][20]; + nextP[10][21] = P[10][21]; + nextP[10][22] = P[10][22]; + nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6]; + nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2; + nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2; + nextP[11][3] = P[11][3] + P[11][0]*SF[5] + P[11][1]*SF[4] + P[11][2]*SF[7] - P[11][11]*SF[11] + P[11][10]*SPP[7] - (P[11][12]*q0)/2; + nextP[11][4] = P[11][4] + P[11][1]*SF[1] + P[11][0]*SF[3] + P[11][2]*SPP[0] - P[11][3]*SPP[2] - P[11][13]*SPP[4]; + nextP[11][5] = P[11][5] + P[11][0]*SF[2] + P[11][2]*SF[1] + P[11][3]*SF[3] - P[11][1]*SPP[0] + P[11][13]*SPP[3]; + nextP[11][6] = P[11][6] + P[11][1]*SF[2] + P[11][3]*SF[1] + P[11][0]*SPP[0] - P[11][2]*SPP[1] - P[11][13]*SPP[5]; + nextP[11][7] = P[11][7] + P[11][4]*dt; + nextP[11][8] = P[11][8] + P[11][5]*dt; + nextP[11][9] = P[11][9] + P[11][6]*dt; + nextP[11][10] = P[11][10]; + nextP[11][11] = P[11][11]; + nextP[11][12] = P[11][12]; + nextP[11][13] = P[11][13]; + nextP[11][14] = P[11][14]; + nextP[11][15] = P[11][15]; + nextP[11][16] = P[11][16]; + nextP[11][17] = P[11][17]; + nextP[11][18] = P[11][18]; + nextP[11][19] = P[11][19]; + nextP[11][20] = P[11][20]; + nextP[11][21] = P[11][21]; + nextP[11][22] = P[11][22]; + nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6]; + nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2; + nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2; + nextP[12][3] = P[12][3] + P[12][0]*SF[5] + P[12][1]*SF[4] + P[12][2]*SF[7] - P[12][11]*SF[11] + P[12][10]*SPP[7] - (P[12][12]*q0)/2; + nextP[12][4] = P[12][4] + P[12][1]*SF[1] + P[12][0]*SF[3] + P[12][2]*SPP[0] - P[12][3]*SPP[2] - P[12][13]*SPP[4]; + nextP[12][5] = P[12][5] + P[12][0]*SF[2] + P[12][2]*SF[1] + P[12][3]*SF[3] - P[12][1]*SPP[0] + P[12][13]*SPP[3]; + nextP[12][6] = P[12][6] + P[12][1]*SF[2] + P[12][3]*SF[1] + P[12][0]*SPP[0] - P[12][2]*SPP[1] - P[12][13]*SPP[5]; + nextP[12][7] = P[12][7] + P[12][4]*dt; + nextP[12][8] = P[12][8] + P[12][5]*dt; + nextP[12][9] = P[12][9] + P[12][6]*dt; + nextP[12][10] = P[12][10]; + nextP[12][11] = P[12][11]; + nextP[12][12] = P[12][12]; + nextP[12][13] = P[12][13]; + nextP[12][14] = P[12][14]; + nextP[12][15] = P[12][15]; + nextP[12][16] = P[12][16]; + nextP[12][17] = P[12][17]; + nextP[12][18] = P[12][18]; + nextP[12][19] = P[12][19]; + nextP[12][20] = P[12][20]; + nextP[12][21] = P[12][21]; + nextP[12][22] = P[12][22]; + nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6]; + nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2; + nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2; + nextP[13][3] = P[13][3] + P[13][0]*SF[5] + P[13][1]*SF[4] + P[13][2]*SF[7] - P[13][11]*SF[11] + P[13][10]*SPP[7] - (P[13][12]*q0)/2; + nextP[13][4] = P[13][4] + P[13][1]*SF[1] + P[13][0]*SF[3] + P[13][2]*SPP[0] - P[13][3]*SPP[2] - P[13][13]*SPP[4]; + nextP[13][5] = P[13][5] + P[13][0]*SF[2] + P[13][2]*SF[1] + P[13][3]*SF[3] - P[13][1]*SPP[0] + P[13][13]*SPP[3]; + nextP[13][6] = P[13][6] + P[13][1]*SF[2] + P[13][3]*SF[1] + P[13][0]*SPP[0] - P[13][2]*SPP[1] - P[13][13]*SPP[5]; + nextP[13][7] = P[13][7] + P[13][4]*dt; + nextP[13][8] = P[13][8] + P[13][5]*dt; + nextP[13][9] = P[13][9] + P[13][6]*dt; + nextP[13][10] = P[13][10]; + nextP[13][11] = P[13][11]; + nextP[13][12] = P[13][12]; + nextP[13][13] = P[13][13]; + nextP[13][14] = P[13][14]; + nextP[13][15] = P[13][15]; + nextP[13][16] = P[13][16]; + nextP[13][17] = P[13][17]; + nextP[13][18] = P[13][18]; + nextP[13][19] = P[13][19]; + nextP[13][20] = P[13][20]; + nextP[13][21] = P[13][21]; + nextP[13][22] = P[13][22]; + nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6]; + nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2; + nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2; + nextP[14][3] = P[14][3] + P[14][0]*SF[5] + P[14][1]*SF[4] + P[14][2]*SF[7] - P[14][11]*SF[11] + P[14][10]*SPP[7] - (P[14][12]*q0)/2; + nextP[14][4] = P[14][4] + P[14][1]*SF[1] + P[14][0]*SF[3] + P[14][2]*SPP[0] - P[14][3]*SPP[2] - P[14][13]*SPP[4]; + nextP[14][5] = P[14][5] + P[14][0]*SF[2] + P[14][2]*SF[1] + P[14][3]*SF[3] - P[14][1]*SPP[0] + P[14][13]*SPP[3]; + nextP[14][6] = P[14][6] + P[14][1]*SF[2] + P[14][3]*SF[1] + P[14][0]*SPP[0] - P[14][2]*SPP[1] - P[14][13]*SPP[5]; + nextP[14][7] = P[14][7] + P[14][4]*dt; + nextP[14][8] = P[14][8] + P[14][5]*dt; + nextP[14][9] = P[14][9] + P[14][6]*dt; + nextP[14][10] = P[14][10]; + nextP[14][11] = P[14][11]; + nextP[14][12] = P[14][12]; + nextP[14][13] = P[14][13]; + nextP[14][14] = P[14][14]; + nextP[14][15] = P[14][15]; + nextP[14][16] = P[14][16]; + nextP[14][17] = P[14][17]; + nextP[14][18] = P[14][18]; + nextP[14][19] = P[14][19]; + nextP[14][20] = P[14][20]; + nextP[14][21] = P[14][21]; + nextP[14][22] = P[14][22]; + nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6]; + nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2; + nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2; + nextP[15][3] = P[15][3] + P[15][0]*SF[5] + P[15][1]*SF[4] + P[15][2]*SF[7] - P[15][11]*SF[11] + P[15][10]*SPP[7] - (P[15][12]*q0)/2; + nextP[15][4] = P[15][4] + P[15][1]*SF[1] + P[15][0]*SF[3] + P[15][2]*SPP[0] - P[15][3]*SPP[2] - P[15][13]*SPP[4]; + nextP[15][5] = P[15][5] + P[15][0]*SF[2] + P[15][2]*SF[1] + P[15][3]*SF[3] - P[15][1]*SPP[0] + P[15][13]*SPP[3]; + nextP[15][6] = P[15][6] + P[15][1]*SF[2] + P[15][3]*SF[1] + P[15][0]*SPP[0] - P[15][2]*SPP[1] - P[15][13]*SPP[5]; + nextP[15][7] = P[15][7] + P[15][4]*dt; + nextP[15][8] = P[15][8] + P[15][5]*dt; + nextP[15][9] = P[15][9] + P[15][6]*dt; + nextP[15][10] = P[15][10]; + nextP[15][11] = P[15][11]; + nextP[15][12] = P[15][12]; + nextP[15][13] = P[15][13]; + nextP[15][14] = P[15][14]; + nextP[15][15] = P[15][15]; + nextP[15][16] = P[15][16]; + nextP[15][17] = P[15][17]; + nextP[15][18] = P[15][18]; + nextP[15][19] = P[15][19]; + nextP[15][20] = P[15][20]; + nextP[15][21] = P[15][21]; + nextP[15][22] = P[15][22]; + nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6]; + nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2; + nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2; + nextP[16][3] = P[16][3] + P[16][0]*SF[5] + P[16][1]*SF[4] + P[16][2]*SF[7] - P[16][11]*SF[11] + P[16][10]*SPP[7] - (P[16][12]*q0)/2; + nextP[16][4] = P[16][4] + P[16][1]*SF[1] + P[16][0]*SF[3] + P[16][2]*SPP[0] - P[16][3]*SPP[2] - P[16][13]*SPP[4]; + nextP[16][5] = P[16][5] + P[16][0]*SF[2] + P[16][2]*SF[1] + P[16][3]*SF[3] - P[16][1]*SPP[0] + P[16][13]*SPP[3]; + nextP[16][6] = P[16][6] + P[16][1]*SF[2] + P[16][3]*SF[1] + P[16][0]*SPP[0] - P[16][2]*SPP[1] - P[16][13]*SPP[5]; + nextP[16][7] = P[16][7] + P[16][4]*dt; + nextP[16][8] = P[16][8] + P[16][5]*dt; + nextP[16][9] = P[16][9] + P[16][6]*dt; + nextP[16][10] = P[16][10]; + nextP[16][11] = P[16][11]; + nextP[16][12] = P[16][12]; + nextP[16][13] = P[16][13]; + nextP[16][14] = P[16][14]; + nextP[16][15] = P[16][15]; + nextP[16][16] = P[16][16]; + nextP[16][17] = P[16][17]; + nextP[16][18] = P[16][18]; + nextP[16][19] = P[16][19]; + nextP[16][20] = P[16][20]; + nextP[16][21] = P[16][21]; + nextP[16][22] = P[16][22]; + nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6]; + nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2; + nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2; + nextP[17][3] = P[17][3] + P[17][0]*SF[5] + P[17][1]*SF[4] + P[17][2]*SF[7] - P[17][11]*SF[11] + P[17][10]*SPP[7] - (P[17][12]*q0)/2; + nextP[17][4] = P[17][4] + P[17][1]*SF[1] + P[17][0]*SF[3] + P[17][2]*SPP[0] - P[17][3]*SPP[2] - P[17][13]*SPP[4]; + nextP[17][5] = P[17][5] + P[17][0]*SF[2] + P[17][2]*SF[1] + P[17][3]*SF[3] - P[17][1]*SPP[0] + P[17][13]*SPP[3]; + nextP[17][6] = P[17][6] + P[17][1]*SF[2] + P[17][3]*SF[1] + P[17][0]*SPP[0] - P[17][2]*SPP[1] - P[17][13]*SPP[5]; + nextP[17][7] = P[17][7] + P[17][4]*dt; + nextP[17][8] = P[17][8] + P[17][5]*dt; + nextP[17][9] = P[17][9] + P[17][6]*dt; + nextP[17][10] = P[17][10]; + nextP[17][11] = P[17][11]; + nextP[17][12] = P[17][12]; + nextP[17][13] = P[17][13]; + nextP[17][14] = P[17][14]; + nextP[17][15] = P[17][15]; + nextP[17][16] = P[17][16]; + nextP[17][17] = P[17][17]; + nextP[17][18] = P[17][18]; + nextP[17][19] = P[17][19]; + nextP[17][20] = P[17][20]; + nextP[17][21] = P[17][21]; + nextP[17][22] = P[17][22]; + nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6]; + nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2; + nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2; + nextP[18][3] = P[18][3] + P[18][0]*SF[5] + P[18][1]*SF[4] + P[18][2]*SF[7] - P[18][11]*SF[11] + P[18][10]*SPP[7] - (P[18][12]*q0)/2; + nextP[18][4] = P[18][4] + P[18][1]*SF[1] + P[18][0]*SF[3] + P[18][2]*SPP[0] - P[18][3]*SPP[2] - P[18][13]*SPP[4]; + nextP[18][5] = P[18][5] + P[18][0]*SF[2] + P[18][2]*SF[1] + P[18][3]*SF[3] - P[18][1]*SPP[0] + P[18][13]*SPP[3]; + nextP[18][6] = P[18][6] + P[18][1]*SF[2] + P[18][3]*SF[1] + P[18][0]*SPP[0] - P[18][2]*SPP[1] - P[18][13]*SPP[5]; + nextP[18][7] = P[18][7] + P[18][4]*dt; + nextP[18][8] = P[18][8] + P[18][5]*dt; + nextP[18][9] = P[18][9] + P[18][6]*dt; + nextP[18][10] = P[18][10]; + nextP[18][11] = P[18][11]; + nextP[18][12] = P[18][12]; + nextP[18][13] = P[18][13]; + nextP[18][14] = P[18][14]; + nextP[18][15] = P[18][15]; + nextP[18][16] = P[18][16]; + nextP[18][17] = P[18][17]; + nextP[18][18] = P[18][18]; + nextP[18][19] = P[18][19]; + nextP[18][20] = P[18][20]; + nextP[18][21] = P[18][21]; + nextP[18][22] = P[18][22]; + nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6]; + nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2; + nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2; + nextP[19][3] = P[19][3] + P[19][0]*SF[5] + P[19][1]*SF[4] + P[19][2]*SF[7] - P[19][11]*SF[11] + P[19][10]*SPP[7] - (P[19][12]*q0)/2; + nextP[19][4] = P[19][4] + P[19][1]*SF[1] + P[19][0]*SF[3] + P[19][2]*SPP[0] - P[19][3]*SPP[2] - P[19][13]*SPP[4]; + nextP[19][5] = P[19][5] + P[19][0]*SF[2] + P[19][2]*SF[1] + P[19][3]*SF[3] - P[19][1]*SPP[0] + P[19][13]*SPP[3]; + nextP[19][6] = P[19][6] + P[19][1]*SF[2] + P[19][3]*SF[1] + P[19][0]*SPP[0] - P[19][2]*SPP[1] - P[19][13]*SPP[5]; + nextP[19][7] = P[19][7] + P[19][4]*dt; + nextP[19][8] = P[19][8] + P[19][5]*dt; + nextP[19][9] = P[19][9] + P[19][6]*dt; + nextP[19][10] = P[19][10]; + nextP[19][11] = P[19][11]; + nextP[19][12] = P[19][12]; + nextP[19][13] = P[19][13]; + nextP[19][14] = P[19][14]; + nextP[19][15] = P[19][15]; + nextP[19][16] = P[19][16]; + nextP[19][17] = P[19][17]; + nextP[19][18] = P[19][18]; + nextP[19][19] = P[19][19]; + nextP[19][20] = P[19][20]; + nextP[19][21] = P[19][21]; + nextP[19][22] = P[19][22]; + nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6]; + nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2; + nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2; + nextP[20][3] = P[20][3] + P[20][0]*SF[5] + P[20][1]*SF[4] + P[20][2]*SF[7] - P[20][11]*SF[11] + P[20][10]*SPP[7] - (P[20][12]*q0)/2; + nextP[20][4] = P[20][4] + P[20][1]*SF[1] + P[20][0]*SF[3] + P[20][2]*SPP[0] - P[20][3]*SPP[2] - P[20][13]*SPP[4]; + nextP[20][5] = P[20][5] + P[20][0]*SF[2] + P[20][2]*SF[1] + P[20][3]*SF[3] - P[20][1]*SPP[0] + P[20][13]*SPP[3]; + nextP[20][6] = P[20][6] + P[20][1]*SF[2] + P[20][3]*SF[1] + P[20][0]*SPP[0] - P[20][2]*SPP[1] - P[20][13]*SPP[5]; + nextP[20][7] = P[20][7] + P[20][4]*dt; + nextP[20][8] = P[20][8] + P[20][5]*dt; + nextP[20][9] = P[20][9] + P[20][6]*dt; + nextP[20][10] = P[20][10]; + nextP[20][11] = P[20][11]; + nextP[20][12] = P[20][12]; + nextP[20][13] = P[20][13]; + nextP[20][14] = P[20][14]; + nextP[20][15] = P[20][15]; + nextP[20][16] = P[20][16]; + nextP[20][17] = P[20][17]; + nextP[20][18] = P[20][18]; + nextP[20][19] = P[20][19]; + nextP[20][20] = P[20][20]; + nextP[20][21] = P[20][21]; + nextP[20][22] = P[20][22]; + nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6]; + nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2; + nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2; + nextP[21][3] = P[21][3] + P[21][0]*SF[5] + P[21][1]*SF[4] + P[21][2]*SF[7] - P[21][11]*SF[11] + P[21][10]*SPP[7] - (P[21][12]*q0)/2; + nextP[21][4] = P[21][4] + P[21][1]*SF[1] + P[21][0]*SF[3] + P[21][2]*SPP[0] - P[21][3]*SPP[2] - P[21][13]*SPP[4]; + nextP[21][5] = P[21][5] + P[21][0]*SF[2] + P[21][2]*SF[1] + P[21][3]*SF[3] - P[21][1]*SPP[0] + P[21][13]*SPP[3]; + nextP[21][6] = P[21][6] + P[21][1]*SF[2] + P[21][3]*SF[1] + P[21][0]*SPP[0] - P[21][2]*SPP[1] - P[21][13]*SPP[5]; + nextP[21][7] = P[21][7] + P[21][4]*dt; + nextP[21][8] = P[21][8] + P[21][5]*dt; + nextP[21][9] = P[21][9] + P[21][6]*dt; + nextP[21][10] = P[21][10]; + nextP[21][11] = P[21][11]; + nextP[21][12] = P[21][12]; + nextP[21][13] = P[21][13]; + nextP[21][14] = P[21][14]; + nextP[21][15] = P[21][15]; + nextP[21][16] = P[21][16]; + nextP[21][17] = P[21][17]; + nextP[21][18] = P[21][18]; + nextP[21][19] = P[21][19]; + nextP[21][20] = P[21][20]; + nextP[21][21] = P[21][21]; + nextP[21][22] = P[21][22]; + nextP[22][0] = P[22][0] + P[22][1]*SF[7] + P[22][2]*SF[9] + P[22][3]*SF[8] + P[22][10]*SF[11] + P[22][11]*SPP[7] + P[22][12]*SPP[6]; + nextP[22][1] = P[22][1] + P[22][0]*SF[6] + P[22][2]*SF[5] + P[22][3]*SF[9] + P[22][11]*SPP[6] - P[22][12]*SPP[7] - (P[22][10]*q0)/2; + nextP[22][2] = P[22][2] + P[22][0]*SF[4] + P[22][1]*SF[8] + P[22][3]*SF[6] + P[22][12]*SF[11] - P[22][10]*SPP[6] - (P[22][11]*q0)/2; + nextP[22][3] = P[22][3] + P[22][0]*SF[5] + P[22][1]*SF[4] + P[22][2]*SF[7] - P[22][11]*SF[11] + P[22][10]*SPP[7] - (P[22][12]*q0)/2; + nextP[22][4] = P[22][4] + P[22][1]*SF[1] + P[22][0]*SF[3] + P[22][2]*SPP[0] - P[22][3]*SPP[2] - P[22][13]*SPP[4]; + nextP[22][5] = P[22][5] + P[22][0]*SF[2] + P[22][2]*SF[1] + P[22][3]*SF[3] - P[22][1]*SPP[0] + P[22][13]*SPP[3]; + nextP[22][6] = P[22][6] + P[22][1]*SF[2] + P[22][3]*SF[1] + P[22][0]*SPP[0] - P[22][2]*SPP[1] - P[22][13]*SPP[5]; + nextP[22][7] = P[22][7] + P[22][4]*dt; + nextP[22][8] = P[22][8] + P[22][5]*dt; + nextP[22][9] = P[22][9] + P[22][6]*dt; + nextP[22][10] = P[22][10]; + nextP[22][11] = P[22][11]; + nextP[22][12] = P[22][12]; + nextP[22][13] = P[22][13]; + nextP[22][14] = P[22][14]; + nextP[22][15] = P[22][15]; + nextP[22][16] = P[22][16]; + nextP[22][17] = P[22][17]; + nextP[22][18] = P[22][18]; + nextP[22][19] = P[22][19]; + nextP[22][20] = P[22][20]; + nextP[22][21] = P[22][21]; + nextP[22][22] = P[22][22]; + + for (unsigned i = 0; i < n_states; i++) + { + nextP[i][i] = nextP[i][i] + processNoise[i]; + } + + // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by + // setting the coresponding covariance terms to zero. + if (onGround || !useCompass) + { + zeroRows(nextP,16,21); + zeroCols(nextP,16,21); + } + + // If on ground or not using airspeed sensing, inhibit wind velocity + // covariance growth. + if (onGround || !useAirspeed) + { + zeroRows(nextP,14,15); + zeroCols(nextP,14,15); + } + + // If on ground, inhibit terrain offset updates by + // setting the coresponding covariance terms to zero. + if (onGround) + { + zeroRows(nextP,22,22); + zeroCols(nextP,22,22); + } + + // If the total position variance exceds 1E6 (1000m), then stop covariance + // growth by setting the predicted to the previous values + // This prevent an ill conditioned matrix from occurring for long periods + // without GPS + if ((P[7][7] + P[8][8]) > 1E6f) + { + for (uint8_t i=7; i<=8; i++) + { + for (unsigned j = 0; j < n_states; j++) + { + nextP[i][j] = P[i][j]; + nextP[j][i] = P[j][i]; + } + } + } + + if (onGround || staticMode) { + // copy the portion of the variances we want to + // propagate + for (unsigned i = 0; i <= 13; i++) { + P[i][i] = nextP[i][i]; + + // force symmetry for observable states + // force zero for non-observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + if ((i > 13) || (j > 13)) { + P[i][j] = 0.0f; + } else { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + } + P[j][i] = P[i][j]; + } + } + } + + } else { + + // Copy covariance + for (unsigned i = 0; i < n_states; i++) { + P[i][i] = nextP[i][i]; + } + + // force symmetry for observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; + } + } + + } + + ConstrainVariances(); +} + +void AttPosEKF::FuseVelposNED() +{ + +// declare variables used by fault isolation logic + uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure + uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available + uint32_t hgtRetryTime = 5000; // height measurement retry time + uint32_t horizRetryTime; + +// declare variables used to check measurement errors + float velInnov[3] = {0.0f,0.0f,0.0f}; + float posInnov[2] = {0.0f,0.0f}; + float hgtInnov = 0.0f; + +// declare variables used to control access to arrays + bool fuseData[6] = {false,false,false,false,false,false}; + uint8_t stateIndex; + uint8_t obsIndex; + uint8_t indexLimit; + +// declare variables used by state and covariance update calculations + float velErr; + float posErr; + float R_OBS[6]; + float observation[6]; + float SK; + float quatMag; + +// Perform sequential fusion of GPS measurements. This assumes that the +// errors in the different velocity and position components are +// uncorrelated which is not true, however in the absence of covariance +// data from the GPS receiver it is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion + if (fuseVelData || fusePosData || fuseHgtData) + { + // set the GPS data timeout depending on whether airspeed data is present + if (useAirspeed) horizRetryTime = gpsRetryTime; + else horizRetryTime = gpsRetryTimeNoTAS; + + // Form the observation vector + for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; + for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; + observation[5] = -(hgtMea); + + // Estimate the GPS Velocity, GPS horiz position and height measurement variances. + velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring + posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring + R_OBS[0] = sq(vneSigma) + sq(velErr); + R_OBS[1] = R_OBS[0]; + R_OBS[2] = sq(vdSigma) + sq(velErr); + R_OBS[3] = sq(posNeSigma) + sq(posErr); + R_OBS[4] = R_OBS[3]; + R_OBS[5] = sq(posDSigma) + sq(posErr); + + // Set innovation variances to zero default + for (uint8_t i = 0; i<=5; i++) + { + varInnovVelPos[i] = 0.0f; + } + // calculate innovations and check GPS data validity using an innovation consistency check + if (fuseVelData) + { + // test velocity measurements + uint8_t imax = 2; + if (fusionModeGPS == 1) imax = 1; + for (uint8_t i = 0; i<=imax; i++) + { + velInnov[i] = statesAtVelTime[i+4] - velNED[i]; + stateIndex = 4 + i; + varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; + } + // apply a 5-sigma threshold + current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); + current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; + if (current_ekf_state.velHealth || current_ekf_state.velTimeout) + { + current_ekf_state.velHealth = true; + current_ekf_state.velFailTime = millis(); + } + else + { + current_ekf_state.velHealth = false; + } + } + if (fusePosData) + { + // test horizontal position measurements + posInnov[0] = statesAtPosTime[7] - posNE[0]; + posInnov[1] = statesAtPosTime[8] - posNE[1]; + varInnovVelPos[3] = P[7][7] + R_OBS[3]; + varInnovVelPos[4] = P[8][8] + R_OBS[4]; + // apply a 10-sigma threshold + current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); + current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; + if (current_ekf_state.posHealth || current_ekf_state.posTimeout) + { + current_ekf_state.posHealth = true; + current_ekf_state.posFailTime = millis(); + } + else + { + current_ekf_state.posHealth = false; + } + } + // test height measurements + if (fuseHgtData) + { + hgtInnov = statesAtHgtTime[9] + hgtMea; + varInnovVelPos[5] = P[9][9] + R_OBS[5]; + // apply a 10-sigma threshold + current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; + current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) + { + current_ekf_state.hgtHealth = true; + current_ekf_state.hgtFailTime = millis(); + } + else + { + current_ekf_state.hgtHealth = false; + } + } + // Set range for sequential fusion of velocity and position measurements depending + // on which data is available and its health + if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + fuseData[2] = true; + } + if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + } + if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) + { + fuseData[3] = true; + fuseData[4] = true; + } + if (fuseHgtData && current_ekf_state.hgtHealth) + { + fuseData[5] = true; + } + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 22; + } + else + { + indexLimit = 13; + } + // Fuse measurements sequentially + for (obsIndex=0; obsIndex<=5; obsIndex++) + { + if (fuseData[obsIndex]) + { + stateIndex = 4 + obsIndex; + // Calculate the measurement innovation, using states from a + // different time coordinate if fusing height data + if (obsIndex >= 0 && obsIndex <= 2) + { + innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 3 || obsIndex == 4) + { + innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 5) + { + innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; + } + // Calculate the Kalman Gain + // Calculate innovation variances - also used for data logging + varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; + SK = 1.0/varInnovVelPos[obsIndex]; + for (uint8_t i= 0; i<=indexLimit; i++) + { + Kfusion[i] = P[i][stateIndex]*SK; + } + + // Don't update Z accel bias state unless using a height observation (GPS velocities can be biased) + if (obsIndex != 5) { + Kfusion[13] = 0; + } + + // Calculate state corrections and re-normalise the quaternions + for (uint8_t i = 0; i<=indexLimit; i++) + { + states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; + } + quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) // divide by 0 protection + { + for (uint8_t i = 0; i<=3; i++) + { + states[i] = states[i] / quatMag; + } + } + // Update the covariance - take advantage of direct observation of a + // single state at index = stateIndex to reduce computations + // Optimised implementation of standard equation P = (I - K*H)*P; + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + KHP[i][j] = Kfusion[i] * P[stateIndex][j]; + } + } + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + } + + ForceSymmetry(); + ConstrainVariances(); + +} + +void AttPosEKF::FuseMagnetometer() +{ + + float &q0 = magstate.q0; + float &q1 = magstate.q1; + float &q2 = magstate.q2; + float &q3 = magstate.q3; + float &magN = magstate.magN; + float &magE = magstate.magE; + float &magD = magstate.magD; + float &magXbias = magstate.magXbias; + float &magYbias = magstate.magYbias; + float &magZbias = magstate.magZbias; + unsigned &obsIndex = magstate.obsIndex; + Mat3f &DCM = magstate.DCM; + float *MagPred = &magstate.MagPred[0]; + float &R_MAG = magstate.R_MAG; + float *SH_MAG = &magstate.SH_MAG[0]; + + float SK_MX[6]; + float SK_MY[5]; + float SK_MZ[6]; + float H_MAG[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_MAG[i] = 0.0f; + } + unsigned indexLimit; + +// Perform sequential fusion of Magnetometer measurements. +// This assumes that the errors in the different components are +// uncorrelated which is not true, however in the absence of covariance +// data fit is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion + if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) + { + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = n_states; + } + else + { + indexLimit = 13 + 1; + } + + // Sequential fusion of XYZ components to spread processing load across + // three prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseMagData) + { + // Copy required states to local variable names + q0 = statesAtMagMeasTime[0]; + q1 = statesAtMagMeasTime[1]; + q2 = statesAtMagMeasTime[2]; + q3 = statesAtMagMeasTime[3]; + magN = statesAtMagMeasTime[16]; + magE = statesAtMagMeasTime[17]; + magD = statesAtMagMeasTime[18]; + magXbias = statesAtMagMeasTime[19]; + magYbias = statesAtMagMeasTime[20]; + magZbias = statesAtMagMeasTime[21]; + + // rotate predicted earth components into body axes and calculate + // predicted measurments + DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3; + DCM.x.y = 2*(q1*q2 + q0*q3); + DCM.x.z = 2*(q1*q3-q0*q2); + DCM.y.x = 2*(q1*q2 - q0*q3); + DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3; + DCM.y.z = 2*(q2*q3 + q0*q1); + DCM.z.x = 2*(q1*q3 + q0*q2); + DCM.z.y = 2*(q2*q3 - q0*q1); + DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3; + MagPred[0] = DCM.x.x*magN + DCM.x.y*magE + DCM.x.z*magD + magXbias; + MagPred[1] = DCM.y.x*magN + DCM.y.y*magE + DCM.y.z*magD + magYbias; + MagPred[2] = DCM.z.x*magN + DCM.z.y*magE + DCM.z.z*magD + magZbias; + + // scale magnetometer observation error with total angular rate + R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU); + + // Calculate observation jacobians + SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; + SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SH_MAG[3] = sq(q3); + SH_MAG[4] = sq(q2); + SH_MAG[5] = sq(q1); + SH_MAG[6] = sq(q0); + SH_MAG[7] = 2*magN*q0; + SH_MAG[8] = 2*magE*q3; + + for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[1] = SH_MAG[0]; + H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; + H_MAG[3] = SH_MAG[2]; + H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[17] = 2*q0*q3 + 2*q1*q2; + H_MAG[18] = 2*q1*q3 - 2*q0*q2; + H_MAG[19] = 1.0f; + + // Calculate Kalman gain + float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + if (temp >= R_MAG) { + SK_MX[0] = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the state variances and try again next time + P[19][19] += 0.1f*R_MAG; + obsIndex = 1; + return; + } + SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; + SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MX[4] = 2*q0*q2 - 2*q1*q3; + SK_MX[5] = 2*q0*q3 + 2*q1*q2; + Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]); + Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[5] - P[1][18]*SK_MX[4]); + Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[5] - P[2][18]*SK_MX[4]); + Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[5] - P[3][18]*SK_MX[4]); + Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]); + Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]); + Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]); + Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]); + Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]); + Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]); + Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]); + Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]); + Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]); + Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); + Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); + Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); + Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); + varInnovMag[0] = 1.0f/SK_MX[0]; + innovMag[0] = MagPred[0] - magData.x; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + fuseMagData = false; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (unsigned int i = 0; i < n_states; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[2]; + H_MAG[1] = SH_MAG[1]; + H_MAG[2] = SH_MAG[0]; + H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; + H_MAG[16] = 2*q1*q2 - 2*q0*q3; + H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; + H_MAG[18] = 2*q0*q1 + 2*q2*q3; + H_MAG[20] = 1; + + // Calculate Kalman gain + float temp = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + if (temp >= R_MAG) { + SK_MY[0] = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the state variances and try again next time + P[20][20] += 0.1f*R_MAG; + obsIndex = 2; + return; + } + SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; + SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MY[3] = 2*q0*q3 - 2*q1*q2; + SK_MY[4] = 2*q0*q1 + 2*q2*q3; + Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]); + Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]); + Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]); + Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]); + Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]); + Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]); + Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]); + Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]); + Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]); + Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]); + Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]); + Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]); + Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); + Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); + Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); + Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); + Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); + Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); + Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); + Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); + Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); + Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); + varInnovMag[1] = 1.0f/SK_MY[0]; + innovMag[1] = MagPred[1] - magData.y; + } + else if (obsIndex == 2) // we are now fusing the Z measurement + { + // Calculate observation jacobians + for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[1]; + H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; + H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[3] = SH_MAG[0]; + H_MAG[16] = 2*q0*q2 + 2*q1*q3; + H_MAG[17] = 2*q2*q3 - 2*q0*q1; + H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + H_MAG[21] = 1; + + // Calculate Kalman gain + float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + if (temp >= R_MAG) { + SK_MZ[0] = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the state variances and try again next time + P[21][21] += 0.1f*R_MAG; + obsIndex = 3; + return; + } + SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MZ[4] = 2*q0*q1 - 2*q2*q3; + SK_MZ[5] = 2*q0*q2 + 2*q1*q3; + Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[5] - P[0][17]*SK_MZ[4]); + Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[5] - P[1][17]*SK_MZ[4]); + Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[5] - P[2][17]*SK_MZ[4]); + Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[5] - P[3][17]*SK_MZ[4]); + Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[5] - P[4][17]*SK_MZ[4]); + Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[5] - P[5][17]*SK_MZ[4]); + Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[5] - P[6][17]*SK_MZ[4]); + Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[5] - P[7][17]*SK_MZ[4]); + Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[5] - P[8][17]*SK_MZ[4]); + Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[5] - P[9][17]*SK_MZ[4]); + Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[5] - P[10][17]*SK_MZ[4]); + Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[5] - P[11][17]*SK_MZ[4]); + Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]); + Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]); + Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]); + Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]); + Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]); + Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]); + Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]); + Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]); + Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]); + Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]); + varInnovMag[2] = 1.0f/SK_MZ[0]; + innovMag[2] = MagPred[2] - magData.z; + + } + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) + { + // correct the state vector + for (uint8_t j= 0; j < indexLimit; j++) + { + states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i < indexLimit; i++) + { + for (uint8_t j = 0; j <= 3; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f; + if (!onGround) + { + for (uint8_t j = 16; j <= 21; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + } + else + { + for (uint8_t j = 16; j <= 21; j++) + { + KH[i][j] = 0.0f; + } + } + } + for (uint8_t i = 0; i < indexLimit; i++) + { + for (uint8_t j = 0; j < indexLimit; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 3; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + if (!onGround) + { + for (uint8_t k = 16; k<=21; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + } + } + for (uint8_t i = 0; i < indexLimit; i++) + { + for (uint8_t j = 0; j < indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; + + ForceSymmetry(); + ConstrainVariances(); +} + +void AttPosEKF::FuseAirspeed() +{ + float vn; + float ve; + float vd; + float vwn; + float vwe; + float R_TAS = sq(airspeedMeasurementSigma); + float SH_TAS[3]; + float SK_TAS; + float VtasPred; + + // Copy required states to local variable names + vn = statesAtVtasMeasTime[4]; + ve = statesAtVtasMeasTime[5]; + vd = statesAtVtasMeasTime[6]; + vwn = statesAtVtasMeasTime[14]; + vwe = statesAtVtasMeasTime[15]; + + // Need to check that it is flying before fusing airspeed data + // Calculate the predicted airspeed + VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); + // Perform fusion of True Airspeed measurement + if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) + { + // Calculate observation jacobians + SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; + SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; + + float H_TAS[n_states]; + for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f; + H_TAS[4] = SH_TAS[2]; + H_TAS[5] = SH_TAS[1]; + H_TAS[6] = vd*SH_TAS[0]; + H_TAS[14] = -SH_TAS[2]; + H_TAS[15] = -SH_TAS[1]; + + // Calculate Kalman gains + float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + if (temp >= R_TAS) { + SK_TAS = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the wind state variances and try again next time + P[14][14] += 0.05f*R_TAS; + P[15][15] += 0.05f*R_TAS; + return; + } + Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); + Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); + Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][14]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][15]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); + Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][14]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][15]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); + Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][14]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][15]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); + Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][14]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][15]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); + Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][14]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][15]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); + Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][14]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][15]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); + Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][14]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][15]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); + Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][14]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][15]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); + Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][14]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][15]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); + Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][14]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][15]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); + Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); + Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); + Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); + Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); + Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]); + varInnovVtas = 1.0f/SK_TAS; + + // Calculate the measurement innovation + innovVtas = VtasPred - VtasMeas; + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovVtas*innovVtas*SK_TAS) < 25.0) + { + // correct the state vector + for (uint8_t j=0; j <= 22; j++) + { + states[j] = states[j] - Kfusion[j] * innovVtas; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) + { + for (uint8_t j= 0; j <= 3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in H to reduce the + // number of operations + for (uint8_t i = 0; i <= 22; i++) + { + for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0; + for (uint8_t j = 4; j <= 6; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 7; j <= 13; j++) KH[i][j] = 0.0; + for (uint8_t j = 14; j <= 15; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0; + } + for (uint8_t i = 0; i <= 22; i++) + { + for (uint8_t j = 0; j <= 22; j++) + { + KHP[i][j] = 0.0; + for (uint8_t k = 4; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + for (uint8_t k = 14; k <= 15; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + for (uint8_t i = 0; i <= 22; i++) + { + for (uint8_t j = 0; j <= 22; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + + ForceSymmetry(); + ConstrainVariances(); +} + +void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (row=first; row<=last; row++) + { + for (col=0; col 0.87f) + { + // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset + // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations + SH_RNG[0] = SH_RNG[4]*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); + SH_RNG[1] = pd - ptd; + SH_RNG[2] = 1/sq(SH_RNG[0]); + SH_RNG[3] = 1/SH_RNG[0]; + for (uint8_t i = 0; i < n_states; i++) { + H_RNG[i] = 0.0f; + Kfusion[i] = 0.0f; + } + H_RNG[22] = -SH_RNG[3]; + SK_RNG[0] = 1/(R_RNG + SH_RNG[3]*(P[9][9]*SH_RNG[3] - P[22][9]*SH_RNG[3] + P[0][9]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][9]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][9]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][9]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[3]*(P[9][22]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[0][22]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][22]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][22]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][22]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4])*(P[9][0]*SH_RNG[3] - P[22][0]*SH_RNG[3] + P[0][0]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][0]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][0]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][0]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4])*(P[9][1]*SH_RNG[3] - P[22][1]*SH_RNG[3] + P[0][1]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][1]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][1]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][1]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4])*(P[9][2]*SH_RNG[3] - P[22][2]*SH_RNG[3] + P[0][2]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][2]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][2]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][2]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])*(P[9][3]*SH_RNG[3] - P[22][3]*SH_RNG[3] + P[0][3]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][3]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][3]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][3]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4]))); + SK_RNG[1] = 2*q1 - 2*q3*SH_RNG[4]; + SK_RNG[2] = 2*q0 - 2*q2*SH_RNG[4]; + SK_RNG[3] = 2*q3 + 2*q1*SH_RNG[4]; + SK_RNG[4] = 2*q2 + 2*q0*SH_RNG[4]; + SK_RNG[5] = SH_RNG[2]; + Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); + + // Calculate the measurement innovation + rngPred = (ptd - pd)/cosRngTilt; + innovRng = rngPred - rngMea; + //printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd); + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovRng*innovRng*SK_RNG[0]) < 25) + { + // correct the state vector + states[22] = states[22] - Kfusion[22] * innovRng; + + // correct the covariance P = (I - K*H)*P + P[22][22] = P[22][22] - Kfusion[22] * H_RNG[22] * P[22][22]; + P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); + } + } + +} + +void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (col=first; col<=last; col++) + { + for (row=0; row < n_states; row++) + { + covMat[row][col] = 0.0; + } + } +} + +float AttPosEKF::sq(float valIn) +{ + return valIn*valIn; +} + +// Store states in a history array along with time stamp +void AttPosEKF::StoreStates(uint64_t timestamp_ms) +{ + for (unsigned i=0; i statetimeStamp[storeIndex]) { + timeDelta = msec - statetimeStamp[storeIndex]; + } else { + timeDelta = statetimeStamp[storeIndex] - msec; + } + + if (timeDelta < bestTimeDelta) + { + bestStoreIndex = storeIndex; + bestTimeDelta = timeDelta; + } + } + if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + { + for (unsigned i=0; i < n_states; i++) { + if (isfinite(storedStates[i][bestStoreIndex])) { + statesForFusion[i] = storedStates[i][bestStoreIndex]; + } else if (isfinite(states[i])) { + statesForFusion[i] = states[i]; + } else { + // There is not much we can do here, except reporting the error we just + // found. + ret++; + } + } + } + else // otherwise output current state + { + for (unsigned i = 0; i < n_states; i++) { + if (isfinite(states[i])) { + statesForFusion[i] = states[i]; + } else { + ret++; + } + } + } + + return ret; +} + +void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) +{ + // Calculate the nav to body cosine matrix + float q00 = sq(quat[0]); + float q11 = sq(quat[1]); + float q22 = sq(quat[2]); + float q33 = sq(quat[3]); + float q01 = quat[0]*quat[1]; + float q02 = quat[0]*quat[2]; + float q03 = quat[0]*quat[3]; + float q12 = quat[1]*quat[2]; + float q13 = quat[1]*quat[3]; + float q23 = quat[2]*quat[3]; + + Tnb.x.x = q00 + q11 - q22 - q33; + Tnb.y.y = q00 - q11 + q22 - q33; + Tnb.z.z = q00 - q11 - q22 + q33; + Tnb.y.x = 2*(q12 - q03); + Tnb.z.x = 2*(q13 + q02); + Tnb.x.y = 2*(q12 + q03); + Tnb.z.y = 2*(q23 - q01); + Tnb.x.z = 2*(q13 - q02); + Tnb.y.z = 2*(q23 + q01); +} + +void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) +{ + // Calculate the body to nav cosine matrix + float q00 = sq(quat[0]); + float q11 = sq(quat[1]); + float q22 = sq(quat[2]); + float q33 = sq(quat[3]); + float q01 = quat[0]*quat[1]; + float q02 = quat[0]*quat[2]; + float q03 = quat[0]*quat[3]; + float q12 = quat[1]*quat[2]; + float q13 = quat[1]*quat[3]; + float q23 = quat[2]*quat[3]; + + Tbn.x.x = q00 + q11 - q22 - q33; + Tbn.y.y = q00 - q11 + q22 - q33; + Tbn.z.z = q00 - q11 - q22 + q33; + Tbn.x.y = 2*(q12 - q03); + Tbn.x.z = 2*(q13 + q02); + Tbn.y.x = 2*(q12 + q03); + Tbn.y.z = 2*(q23 - q01); + Tbn.z.x = 2*(q13 - q02); + Tbn.z.y = 2*(q23 + q01); +} + +void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3]) +{ + float u1 = cos(0.5f*eul[0]); + float u2 = cos(0.5f*eul[1]); + float u3 = cos(0.5f*eul[2]); + float u4 = sin(0.5f*eul[0]); + float u5 = sin(0.5f*eul[1]); + float u6 = sin(0.5f*eul[2]); + quat[0] = u1*u2*u3+u4*u5*u6; + quat[1] = u4*u2*u3-u1*u5*u6; + quat[2] = u1*u5*u3+u4*u2*u6; + quat[3] = u1*u2*u6-u4*u5*u3; +} + +void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) +{ + y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3])); + y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2])); + y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); +} + +void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) +{ + velNED[0] = gpsGndSpd*cosf(gpsCourse); + velNED[1] = gpsGndSpd*sinf(gpsCourse); + velNED[2] = gpsVelD; +} + +void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef) +{ + posNED[0] = earthRadius * (lat - latRef); + posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); + posNED[2] = -(hgt - hgtRef); +} + +void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +{ + lat = latRef + posNED[0] * earthRadiusInv; + lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); + hgt = hgtRef - posNED[2]; +} + +void AttPosEKF::OnGroundCheck() +{ + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); + if (staticMode) { + staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); + } +} + +void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) +{ + //Define Earth rotation vector in the NED navigation frame + omega.x = earthRate*cosf(latitude); + omega.y = 0.0f; + omega.z = -earthRate*sinf(latitude); +} + +void AttPosEKF::CovarianceInit() +{ + // Calculate the initial covariance matrix P + P[0][0] = 0.25f * sq(1.0f*deg2rad); + P[1][1] = 0.25f * sq(1.0f*deg2rad); + P[2][2] = 0.25f * sq(1.0f*deg2rad); + P[3][3] = 0.25f * sq(10.0f*deg2rad); + P[4][4] = sq(0.7f); + P[5][5] = P[4][4]; + P[6][6] = sq(0.7f); + P[7][7] = sq(15.0f); + P[8][8] = P[7][7]; + P[9][9] = sq(5.0f); + P[10][10] = sq(0.1f*deg2rad*dtIMU); + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; + P[13][13] = sq(0.2f*dtIMU); + P[14][14] = sq(8.0f); + P[15][14] = P[14][14]; + P[16][16] = sq(0.02f); + P[17][17] = P[16][16]; + P[18][18] = P[16][16]; + P[19][19] = sq(0.02f); + P[20][20] = P[19][19]; + P[21][21] = P[19][19]; + P[22][22] = sq(0.5f); +} + +float AttPosEKF::ConstrainFloat(float val, float min, float max) +{ + float ret; + if (val > max) { + ret = max; + ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val); + } else if (val < min) { + ret = min; + ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val); + } else { + ret = val; + } + + if (!isfinite(val)) { + ekf_debug("constrain: non-finite!"); + } + + return ret; +} + +void AttPosEKF::ConstrainVariances() +{ + if (!numericalProtection) { + return; + } + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13: Delta Velocity bias - m/s (Z) + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + // 22: Terrain offset - m + + // Constrain quaternion variances + for (unsigned i = 0; i <= 3; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Constrain velocity variances + for (unsigned i = 4; i <= 6; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Constrain position variances + for (unsigned i = 7; i <= 9; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); + } + + // Constrain delta angle bias variances + for (unsigned i = 10; i <= 12; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU)); + } + + // Constrain delta velocity bias variance + P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU)); + + // Wind velocity variances + for (unsigned i = 14; i <= 15; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Earth magnetic field variances + for (unsigned i = 16; i <= 18; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Body magnetic field variances + for (unsigned i = 19; i <= 21; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Constrain terrain offset variance + P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); +} + +void AttPosEKF::ConstrainStates() +{ + if (!numericalProtection) { + return; + } + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13: Delta Velocity bias - m/s (Z) + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + // 22: Terrain offset - m + + // Constrain quaternion + for (unsigned i = 0; i <= 3; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Constrain velocities to what GPS can do for us + for (unsigned i = 4; i <= 6; i++) { + states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); + } + + // Constrain position to a reasonable vehicle range (in meters) + for (unsigned i = 7; i <= 8; i++) { + states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); + } + + // Constrain altitude + states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); + + // Angle bias limit - set to 8 degrees / sec + for (unsigned i = 10; i <= 12; i++) { + states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); + } + + // Constrain delta velocity bias + states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); + + // Wind velocity limits - assume 120 m/s max velocity + for (unsigned i = 14; i <= 15; i++) { + states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); + } + + // Earth magnetic field limits (in Gauss) + for (unsigned i = 16; i <= 18; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Body magnetic field variances (in Gauss). + // the max offset should be in this range. + for (unsigned i = 19; i <= 21; i++) { + states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); + } + + // Constrain terrain offset + states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f); + +} + +void AttPosEKF::ForceSymmetry() +{ + if (!numericalProtection) { + return; + } + + // Force symmetry on the covariance matrix to prevent ill-conditioning + // of the matrix which would cause the filter to blow-up + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (P[i][j] + P[j][i]); + P[j][i] = P[i][j]; + } + } +} + +bool AttPosEKF::FilterHealthy() +{ + if (!statesInitialised) { + return false; + } + + // XXX Check state vector for NaNs and ill-conditioning + + // Check if any of the major inputs timed out + if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { + return false; + } + + // Nothing fired, return ok. + return true; +} + +/** + * Reset the filter position states + * + * This resets the position to the last GPS measurement + * or to zero in case of static position. + */ +void AttPosEKF::ResetPosition(void) +{ + if (staticMode) { + states[7] = 0; + states[8] = 0; + } else if (GPSstatus >= GPS_FIX_3D) { + + // reset the states from the GPS measurements + states[7] = posNE[0]; + states[8] = posNE[1]; + } +} + +/** + * Reset the height state. + * + * This resets the height state with the last altitude measurements + */ +void AttPosEKF::ResetHeight(void) +{ + // write to the state vector + states[9] = -hgtMea; +} + +/** + * Reset the velocity state. + */ +void AttPosEKF::ResetVelocity(void) +{ + if (staticMode) { + states[4] = 0.0f; + states[5] = 0.0f; + states[6] = 0.0f; + } else if (GPSstatus >= GPS_FIX_3D) { + + states[4] = velNED[0]; // north velocity from last reading + states[5] = velNED[1]; // east velocity from last reading + states[6] = velNED[2]; // down velocity from last reading + } +} + + +void AttPosEKF::FillErrorReport(struct ekf_status_report *err) +{ + for (unsigned i = 0; i < n_states; i++) + { + err->states[i] = states[i]; + } + + err->velHealth = current_ekf_state.velHealth; + err->posHealth = current_ekf_state.posHealth; + err->hgtHealth = current_ekf_state.hgtHealth; + err->velTimeout = current_ekf_state.velTimeout; + err->posTimeout = current_ekf_state.posTimeout; + err->hgtTimeout = current_ekf_state.hgtTimeout; +} + +bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { + bool err = false; + + // check all integrators + if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { + err_report->angNaN = true; + ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); + err = true; + goto out; + } // delta angles + + if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { + err_report->angNaN = true; + ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); + err = true; + goto out; + } // delta angles + + if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { + err_report->summedDelVelNaN = true; + ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); + err = true; + goto out; + } // delta velocities + + // check all states and covariance matrices + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + if (!isfinite(KH[i][j])) { + + err_report->KHNaN = true; + err = true; + ekf_debug("KH NaN"); + goto out; + } // intermediate result used for covariance updates + + if (!isfinite(KHP[i][j])) { + + err_report->KHPNaN = true; + err = true; + ekf_debug("KHP NaN"); + goto out; + } // intermediate result used for covariance updates + + if (!isfinite(P[i][j])) { + + err_report->covarianceNaN = true; + err = true; + ekf_debug("P NaN"); + } // covariance matrix + } + + if (!isfinite(Kfusion[i])) { + + err_report->kalmanGainsNaN = true; + ekf_debug("Kfusion NaN"); + err = true; + goto out; + } // Kalman gains + + if (!isfinite(states[i])) { + + err_report->statesNaN = true; + ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); + err = true; + goto out; + } // state matrix + } + +out: + if (err) { + FillErrorReport(err_report); + } + + return err; + +} + +/** + * Check the filter inputs and bound its operational state + * + * This check will reset the filter states if required + * due to a failure of consistency or timeout checks. + * it should be run after the measurement data has been + * updated, but before any of the fusion steps are + * executed. + */ +int AttPosEKF::CheckAndBound() +{ + + // Store the old filter state + bool currStaticMode = staticMode; + + // Reset the filter if the states went NaN + if (StatesNaN(&last_ekf_error)) { + ekf_debug("re-initializing dynamic"); + + InitializeDynamic(velNED, magDeclination); + + return 1; + } + + // Reset the filter if the IMU data is too old + if (dtIMU > 0.3f) { + FillErrorReport(&last_ekf_error); + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + // that's all we can do here, return + return 2; + } + + // Check if we're on ground - this also sets static mode. + OnGroundCheck(); + + // Check if we switched between states + if (currStaticMode != staticMode) { + FillErrorReport(&last_ekf_error); + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + return 3; + } + + // Reset the filter if gyro offsets are excessive + if (fabs(states[10]) > 1.0f || fabsf(states[11]) > 1.0f || fabsf(states[12]) > 1.0f) { + + InitializeDynamic(velNED, magDeclination); + + // that's all we can do here, return + return 4; + } + + return 0; +} + +void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) +{ + float initialRoll, initialPitch; + float cosRoll, sinRoll, cosPitch, sinPitch; + float magX, magY; + float initialHdg, cosHeading, sinHeading; + + initialRoll = atan2f(-ay, -az); + initialPitch = atan2f(ax, -az); + + cosRoll = cosf(initialRoll); + sinRoll = sinf(initialRoll); + cosPitch = cosf(initialPitch); + sinPitch = sinf(initialPitch); + + magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; + + magY = my * cosRoll - mz * sinRoll; + + initialHdg = atan2f(-magY, magX); + /* true heading is the mag heading minus declination */ + initialHdg += declination; + + cosRoll = cosf(initialRoll * 0.5f); + sinRoll = sinf(initialRoll * 0.5f); + + cosPitch = cosf(initialPitch * 0.5f); + sinPitch = sinf(initialPitch * 0.5f); + + cosHeading = cosf(initialHdg * 0.5f); + sinHeading = sinf(initialHdg * 0.5f); + + initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; + initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; + initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; + initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + /* normalize */ + float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]); + + initQuat[0] /= norm; + initQuat[1] /= norm; + initQuat[2] /= norm; + initQuat[3] /= norm; +} + +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) +{ + + // Fill variables with valid data + velNED[0] = initvelNED[0]; + velNED[1] = initvelNED[1]; + velNED[2] = initvelNED[2]; + magDeclination = declination; + + // Calculate initial filter quaternion states from raw measurements + float initQuat[4]; + Vector3f initMagXYZ; + initMagXYZ = magData - magBias; + AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, declination, initQuat); + + // Calculate initial Tbn matrix and rotate Mag measurements into NED + // to set initial NED magnetic field states + Mat3f DCM; + quat2Tbn(DCM, initQuat); + Vector3f initMagNED; + initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; + initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; + initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; + + magstate.q0 = initQuat[0]; + magstate.q1 = initQuat[1]; + magstate.q2 = initQuat[2]; + magstate.q3 = initQuat[3]; + magstate.magN = initMagNED.x; + magstate.magE = initMagNED.y; + magstate.magD = initMagNED.z; + magstate.magXbias = magBias.x; + magstate.magYbias = magBias.y; + magstate.magZbias = magBias.z; + magstate.R_MAG = sq(magMeasurementSigma); + magstate.DCM = DCM; + + // write to state vector + for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions + for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities + for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel + states[16] = initMagNED.x; // Magnetic Field North + states[17] = initMagNED.y; // Magnetic Field East + states[18] = initMagNED.z; // Magnetic Field Down + states[19] = magBias.x; // Magnetic Field Bias X + states[20] = magBias.y; // Magnetic Field Bias Y + states[21] = magBias.z; // Magnetic Field Bias Z + states[22] = 0.0f; // terrain height + + ResetVelocity(); + ResetPosition(); + ResetHeight(); + + statesInitialised = true; + + // initialise the covariance matrix + CovarianceInit(); + + //Define Earth rotation vector in the NED navigation frame + calcEarthRateNED(earthRateNED, latRef); + +} + +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) +{ + // store initial lat,long and height + latRef = referenceLat; + lonRef = referenceLon; + hgtRef = referenceHgt; + refSet = true; + + // we are at reference altitude, so measurement must be zero + hgtMea = 0.0f; + posNE[0] = 0.0f; + posNE[1] = 0.0f; + + // the baro offset must be this difference now + baroHgtOffset = baroHgt - referenceHgt; + + InitializeDynamic(initvelNED, declination); +} + +void AttPosEKF::ZeroVariables() +{ + + // Initialize on-init initialized variables + fusionModeGPS = 0; + covSkipCount = 0; + statesInitialised = false; + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + onGround = true; + staticMode = true; + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + numericalProtection = true; + refSet = false; + storeIndex = 0; + gpsHgt = 0.0f; + baroHgt = 0.0f; + GPSstatus = 0; + VtasMeas = 0.0f; + magDeclination = 0.0f; + + // Do the data structure init + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + KH[i][j] = 0.0f; // intermediate result used for covariance updates + KHP[i][j] = 0.0f; // intermediate result used for covariance updates + P[i][j] = 0.0f; // covariance matrix + } + + Kfusion[i] = 0.0f; // Kalman gains + states[i] = 0.0f; // state matrix + } + + correctedDelAng.zero(); + summedDelAng.zero(); + summedDelVel.zero(); + + dAngIMU.zero(); + dVelIMU.zero(); + + for (unsigned i = 0; i < data_buffer_size; i++) { + + for (unsigned j = 0; j < n_states; j++) { + storedStates[j][i] = 0.0f; + } + + statetimeStamp[i] = 0; + } + + memset(&magstate, 0, sizeof(magstate)); + magstate.q0 = 1.0f; + magstate.DCM.identity(); + + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); + +} + +void AttPosEKF::GetFilterState(struct ekf_status_report *state) +{ + + // Copy states + for (unsigned i = 0; i < n_states; i++) { + current_ekf_state.states[i] = states[i]; + } + + memcpy(state, ¤t_ekf_state, sizeof(*state)); +} + +void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) +{ + memcpy(last_error, &last_ekf_error, sizeof(*last_error)); +} diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h new file mode 100644 index 000000000..6d3b89108 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -0,0 +1,282 @@ +#pragma once + +#include "estimator_utilities.h" + +class AttPosEKF { + +public: + + AttPosEKF(); + ~AttPosEKF(); + + + + /* ############################################## + * + * M A I N F I L T E R P A R A M E T E R S + * + * ########################################### */ + + /* + * parameters are defined here and initialised in + * the InitialiseParameters() (which is just 20 lines down) + */ + + float covTimeStepMax; // maximum time allowed between covariance predictions + float covDelAngMax; // maximum delta angle between covariance predictions + float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + + float yawVarScale; + float windVelSigma; + float dAngBiasSigma; + float dVelBiasSigma; + float magEarthSigma; + float magBodySigma; + float gndHgtSigma; + + float vneSigma; + float vdSigma; + float posNeSigma; + float posDSigma; + float magMeasurementSigma; + float airspeedMeasurementSigma; + + float gyroProcessNoise; + float accelProcessNoise; + + float EAS2TAS; // ratio f true to equivalent airspeed + + void InitialiseParameters() + { + covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions + covDelAngMax = 0.02f; // maximum delta angle between covariance predictions + rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + EAS2TAS = 1.0f; + + yawVarScale = 1.0f; + windVelSigma = 0.1f; + dAngBiasSigma = 5.0e-7f; + dVelBiasSigma = 1e-4f; + magEarthSigma = 3.0e-4f; + magBodySigma = 3.0e-4f; + gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma + + vneSigma = 0.2f; + vdSigma = 0.3f; + posNeSigma = 2.0f; + posDSigma = 2.0f; + + magMeasurementSigma = 0.05; + airspeedMeasurementSigma = 1.4f; + gyroProcessNoise = 1.4544411e-2f; + accelProcessNoise = 0.5f; + } + + struct { + unsigned obsIndex; + float MagPred[3]; + float SH_MAG[9]; + float q0; + float q1; + float q2; + float q3; + float magN; + float magE; + float magD; + float magXbias; + float magYbias; + float magZbias; + float R_MAG; + Mat3f DCM; + } magstate; + + + // Global variables + float KH[n_states][n_states]; // intermediate result used for covariance updates + float KHP[n_states][n_states]; // intermediate result used for covariance updates + float P[n_states][n_states]; // covariance matrix + float Kfusion[n_states]; // Kalman gains + float states[n_states]; // state matrix + float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps + uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored + + float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements + float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements + float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement + float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time + float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time + float statesAtRngTime[n_states]; // filter states at the effective measurement time + + Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) + Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) + Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) + Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) + float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) + Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) + Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + + Mat3f Tbn; // transformation matrix from body to NED coordinates + Mat3f Tnb; // transformation amtrix from NED to body coordinates + + Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) + Vector3f dVelIMU; + Vector3f dAngIMU; + float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) + uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity + float innovVelPos[6]; // innovation output + float varInnovVelPos[6]; // innovation variance output + + float velNED[3]; // North, East, Down velocity obs (m/s) + float posNE[2]; // North, East position obs (m) + float hgtMea; // measured height (m) + float baroHgtOffset; ///< the baro (weather) offset from normalized altitude + float rngMea; // Ground distance + + float innovMag[3]; // innovation output + float varInnovMag[3]; // innovation variance output + Vector3f magData; // magnetometer flux radings in X,Y,Z body axes + float innovVtas; // innovation output + float innovRng; ///< Range finder innovation + float varInnovVtas; // innovation variance output + float VtasMeas; // true airspeed measurement (m/s) + float magDeclination; ///< magnetic declination + double latRef; // WGS-84 latitude of reference point (rad) + double lonRef; // WGS-84 longitude of reference point (rad) + float hgtRef; // WGS-84 height of reference point (m) + bool refSet; ///< flag to indicate if the reference position has been set + Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes + unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction + + // GPS input data variables + float gpsCourse; + float gpsVelD; + double gpsLat; + double gpsLon; + float gpsHgt; + uint8_t GPSstatus; + + // Baro input + float baroHgt; + + bool statesInitialised; + + bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused + bool fusePosData; // this boolean causes the posNE and velNED obs to be fused + bool fuseHgtData; // this boolean causes the hgtMea obs to be fused + bool fuseMagData; // boolean true when magnetometer data is to be fused + bool fuseVtasData; // boolean true when airspeed data is to be fused + bool fuseRngData; ///< true when range data is fused + + bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) + bool staticMode; ///< boolean true if no position feedback is fused + bool useAirspeed; ///< boolean true if airspeed data is being used + bool useCompass; ///< boolean true if magnetometer data is being used + bool useRangeFinder; ///< true when rangefinder is being used + + struct ekf_status_report current_ekf_state; + struct ekf_status_report last_ekf_error; + + bool numericalProtection; + + unsigned storeIndex; + + +void UpdateStrapdownEquationsNED(); + +void CovariancePrediction(float dt); + +void FuseVelposNED(); + +void FuseMagnetometer(); + +void FuseAirspeed(); + +void FuseRangeFinder(); + +void FuseOpticalFlow(); + +void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); + +void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); + +void quatNorm(float (&quatOut)[4], const float quatIn[4]); + +// store staes along with system time stamp in msces +void StoreStates(uint64_t timestamp_ms); + +/** + * Recall the state vector. + * + * Recalls the vector stored at closest time to the one specified by msec + * + * @return zero on success, integer indicating the number of invalid states on failure. + * Does only copy valid states, if the statesForFusion vector was initialized + * correctly by the caller, the result can be safely used, but is a mixture + * time-wise where valid states were updated and invalid remained at the old + * value. + */ +int RecallStates(float *statesForFusion, uint64_t msec); + +void ResetStoredStates(); + +void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); + +void calcEarthRateNED(Vector3f &omega, float latitude); + +static void eul2quat(float (&quat)[4], const float (&eul)[3]); + +static void quat2eul(float (&eul)[3], const float (&quat)[4]); + +static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); + +static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); + +static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); + +static float sq(float valIn); + +void OnGroundCheck(); + +void CovarianceInit(); + +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); + +float ConstrainFloat(float val, float min, float max); + +void ConstrainVariances(); + +void ConstrainStates(); + +void ForceSymmetry(); + +int CheckAndBound(); + +void ResetPosition(); + +void ResetVelocity(); + +void ZeroVariables(); + +void GetFilterState(struct ekf_status_report *state); + +void GetLastErrorState(struct ekf_status_report *last_error); + +bool StatesNaN(struct ekf_status_report *err_report); +void FillErrorReport(struct ekf_status_report *err); + +void InitializeDynamic(float (&initvelNED)[3], float declination); + +protected: + +bool FilterHealthy(); + +void ResetHeight(void); + +void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); + +}; + +uint32_t millis(); + -- cgit v1.2.3 From b9a3fa60bc06b31d4862919398c1161e6a35f360 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 13:03:11 +0200 Subject: Add support for 21 and 23 state estimators. Promoto a number of small delta variables to double --- .../ekf_att_pos_estimator_main.cpp | 75 +- .../ekf_att_pos_estimator/estimator_21states.cpp | 2143 ++++++++++++++++++++ .../ekf_att_pos_estimator/estimator_21states.h | 247 +++ .../ekf_att_pos_estimator/estimator_utilities.cpp | 139 ++ .../ekf_att_pos_estimator/estimator_utilities.h | 79 + src/modules/ekf_att_pos_estimator/module.mk | 3 +- 6 files changed, 2660 insertions(+), 26 deletions(-) create mode 100644 src/modules/ekf_att_pos_estimator/estimator_21states.cpp create mode 100644 src/modules/ekf_att_pos_estimator/estimator_21states.h create mode 100644 src/modules/ekf_att_pos_estimator/estimator_utilities.cpp create mode 100644 src/modules/ekf_att_pos_estimator/estimator_utilities.h diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index ee3c60e63..86f7eb99f 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -83,7 +83,7 @@ #include #include -#include "estimator.h" +#include "estimator_21states.h" @@ -1455,30 +1455,55 @@ FixedwingEstimator::print_status() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); - printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); - printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); - printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); - printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); - printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); - printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); - printf("states: %s %s %s %s %s %s %s %s %s %s\n", - (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", - (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", - (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + if (n_states == 23) { + printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); + printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); + printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); + printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); + printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); + printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); + printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); + printf("states: %s %s %s %s %s %s %s %s %s %s\n", + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + } else { + printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); + printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); + printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); + printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); + printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); + printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); + printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); + printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + printf("states: %s %s %s %s %s %s %s %s %s %s\n", + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + } } int FixedwingEstimator::trip_nan() { diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp new file mode 100644 index 000000000..5a60ac1c0 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp @@ -0,0 +1,2143 @@ +#include "estimator_21states.h" + +#include + +AttPosEKF::AttPosEKF() : + fusionModeGPS(0), + covSkipCount(0), + EAS2TAS(1.0f), + statesInitialised(false), + fuseVelData(false), + fusePosData(false), + fuseHgtData(false), + fuseMagData(false), + fuseVtasData(false), + onGround(true), + staticMode(true), + useAirspeed(true), + useCompass(true), + numericalProtection(true), + storeIndex(0), + magDeclination(0.0f) +{ + InitialiseParameters(); +} + +AttPosEKF::~AttPosEKF() +{ +} + +void AttPosEKF::UpdateStrapdownEquationsNED() +{ + Vector3f delVelNav; + float q00; + float q11; + float q22; + float q33; + float q01; + float q02; + float q03; + float q12; + float q13; + float q23; + Mat3f Tbn; + Mat3f Tnb; + float rotationMag; + float qUpdated[4]; + float quatMag; + double deltaQuat[4]; + const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; + +// Remove sensor bias errors + correctedDelAng.x = dAngIMU.x - states[10]; + correctedDelAng.y = dAngIMU.y - states[11]; + correctedDelAng.z = dAngIMU.z - states[12]; + dVelIMU.x = dVelIMU.x; + dVelIMU.y = dVelIMU.y; + dVelIMU.z = dVelIMU.z; + +// Save current measurements + Vector3f prevDelAng = correctedDelAng; + +// Apply corrections for earths rotation rate and coning errors +// * and + operators have been overloaded + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + +// Convert the rotation vector to its equivalent quaternion + rotationMag = correctedDelAng.length(); + if (rotationMag < 1e-12f) + { + deltaQuat[0] = 1.0; + deltaQuat[1] = 0.0; + deltaQuat[2] = 0.0; + deltaQuat[3] = 0.0; + } + else + { + deltaQuat[0] = cos(0.5f*rotationMag); + double rotScaler = (sin(0.5f*rotationMag))/rotationMag; + deltaQuat[1] = correctedDelAng.x*rotScaler; + deltaQuat[2] = correctedDelAng.y*rotScaler; + deltaQuat[3] = correctedDelAng.z*rotScaler; + } + +// Update the quaternions by rotating from the previous attitude through +// the delta angle rotation quaternion + qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; + qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; + qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; + qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; + +// Normalise the quaternions and update the quaternion states + quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); + if (quatMag > 1e-16f) + { + float quatMagInv = 1.0f/quatMag; + states[0] = quatMagInv*qUpdated[0]; + states[1] = quatMagInv*qUpdated[1]; + states[2] = quatMagInv*qUpdated[2]; + states[3] = quatMagInv*qUpdated[3]; + } + +// Calculate the body to nav cosine matrix + q00 = sq(states[0]); + q11 = sq(states[1]); + q22 = sq(states[2]); + q33 = sq(states[3]); + q01 = states[0]*states[1]; + q02 = states[0]*states[2]; + q03 = states[0]*states[3]; + q12 = states[1]*states[2]; + q13 = states[1]*states[3]; + q23 = states[2]*states[3]; + + Tbn.x.x = q00 + q11 - q22 - q33; + Tbn.y.y = q00 - q11 + q22 - q33; + Tbn.z.z = q00 - q11 - q22 + q33; + Tbn.x.y = 2*(q12 - q03); + Tbn.x.z = 2*(q13 + q02); + Tbn.y.x = 2*(q12 + q03); + Tbn.y.z = 2*(q23 - q01); + Tbn.z.x = 2*(q13 - q02); + Tbn.z.y = 2*(q23 + q01); + + Tnb = Tbn.transpose(); + +// transform body delta velocities to delta velocities in the nav frame +// * and + operators have been overloaded + //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; + delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; + delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; + delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; + +// calculate the magnitude of the nav acceleration (required for GPS +// variance estimation) + accNavMag = delVelNav.length()/dtIMU; + +// If calculating position save previous velocity + float lastVelocity[3]; + lastVelocity[0] = states[4]; + lastVelocity[1] = states[5]; + lastVelocity[2] = states[6]; + +// Sum delta velocities to get velocity + states[4] = states[4] + delVelNav.x; + states[5] = states[5] + delVelNav.y; + states[6] = states[6] + delVelNav.z; + +// If calculating postions, do a trapezoidal integration for position + states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; + states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; + states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; + + // Constrain states (to protect against filter divergence) + ConstrainStates(); +} + +void AttPosEKF::CovariancePrediction(float dt) +{ + // scalars + float daxCov; + float dayCov; + float dazCov; + float dvxCov; + float dvyCov; + float dvzCov; + float dvx; + float dvy; + float dvz; + float dax; + float day; + float daz; + float q0; + float q1; + float q2; + float q3; + float dax_b; + float day_b; + float daz_b; + + // arrays + float processNoise[21]; + float SF[14]; + float SG[8]; + float SQ[11]; + float SPP[13] = {0}; + float nextP[21][21]; + + // calculate covariance prediction process noise + windVelSigma = dt*0.1f; + dAngBiasSigma = dt*5.0e-7f; + magEarthSigma = dt*3.0e-4f; + magBodySigma = dt*3.0e-4f; + for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; + if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale; + for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma; + for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma; + for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma; + for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]); + + // set variables used to calculate covariance growth + dvx = summedDelVel.x; + dvy = summedDelVel.y; + dvz = summedDelVel.z; + dax = summedDelAng.x; + day = summedDelAng.y; + daz = summedDelAng.z; + q0 = states[0]; + q1 = states[1]; + q2 = states[2]; + q3 = states[3]; + dax_b = states[10]; + day_b = states[11]; + daz_b = states[12]; + daxCov = sq(dt*1.4544411e-2f); + dayCov = sq(dt*1.4544411e-2f); + dazCov = sq(dt*1.4544411e-2f); + if (onGround) dazCov = dazCov * sq(yawVarScale); + dvxCov = sq(dt*0.5f); + dvyCov = sq(dt*0.5f); + dvzCov = sq(dt*0.5f); + + // Predicted covariance calculation + SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; + SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; + SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; + SF[3] = day/2 - day_b/2; + SF[4] = daz/2 - daz_b/2; + SF[5] = dax/2 - dax_b/2; + SF[6] = dax_b/2 - dax/2; + SF[7] = daz_b/2 - daz/2; + SF[8] = day_b/2 - day/2; + SF[9] = q1/2; + SF[10] = q2/2; + SF[11] = q3/2; + SF[12] = 2*dvz*q0; + SF[13] = 2*dvy*q1; + + SG[0] = q0/2; + SG[1] = sq(q3); + SG[2] = sq(q2); + SG[3] = sq(q1); + SG[4] = sq(q0); + SG[5] = 2*q2*q3; + SG[6] = 2*q1*q3; + SG[7] = 2*q1*q2; + + SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); + SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); + SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); + SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4; + SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4; + SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4; + SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4; + SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2; + SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; + SQ[9] = sq(SG[0]); + SQ[10] = sq(q1); + + SPP[0] = SF[12] + SF[13] - 2*dvx*q2; + SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; + SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; + SPP[3] = SF[11]; + SPP[4] = SF[10]; + SPP[5] = SF[9]; + SPP[6] = SF[7]; + SPP[7] = SF[8]; + + nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2; + nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]); + nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]); + nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]); + nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]); + nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]); + nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]); + nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]; + nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]; + nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]; + nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3]; + nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3]; + nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3]; + nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3]; + nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3]; + nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3]; + nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3]; + nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3]; + nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2); + nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2; + nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2; + nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2; + nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2); + nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2); + nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2); + nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2); + nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2); + nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2); + nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2; + nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2; + nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2; + nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2; + nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2; + nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2; + nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2; + nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2; + nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2; + nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2; + nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2; + nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2); + nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2; + nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2; + nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2; + nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2); + nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2; + nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2; + nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2; + nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2; + nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2; + nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2; + nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2; + nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2; + nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2; + nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2; + nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2; + nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2); + nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2; + nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2; + nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2; + nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2); + nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2; + nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2; + nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2; + nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2; + nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2; + nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2; + nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2; + nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2; + nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2; + nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2; + nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2; + nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]); + nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2; + nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2; + nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2; + nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]); + nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]); + nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]); + nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]); + nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]; + nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]; + nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]; + nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2]; + nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2]; + nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2]; + nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2]; + nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2]; + nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2]; + nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2]; + nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2]; + nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]); + nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2; + nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2; + nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2; + nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]); + nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]); + nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]); + nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]); + nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]); + nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]; + nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]; + nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]; + nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0]; + nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0]; + nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0]; + nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0]; + nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0]; + nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0]; + nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0]; + nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0]; + nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]); + nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2; + nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2; + nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2; + nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]); + nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]); + nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]); + nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]); + nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]); + nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]; + nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]; + nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]; + nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1]; + nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1]; + nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1]; + nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1]; + nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1]; + nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1]; + nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1]; + nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1]; + nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt); + nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; + nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; + nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; + nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt); + nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt); + nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt); + nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); + nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); + nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); + nextP[7][10] = P[7][10] + P[4][10]*dt; + nextP[7][11] = P[7][11] + P[4][11]*dt; + nextP[7][12] = P[7][12] + P[4][12]*dt; + nextP[7][13] = P[7][13] + P[4][13]*dt; + nextP[7][14] = P[7][14] + P[4][14]*dt; + nextP[7][15] = P[7][15] + P[4][15]*dt; + nextP[7][16] = P[7][16] + P[4][16]*dt; + nextP[7][17] = P[7][17] + P[4][17]*dt; + nextP[7][18] = P[7][18] + P[4][18]*dt; + nextP[7][19] = P[7][19] + P[4][19]*dt; + nextP[7][20] = P[7][20] + P[4][20]*dt; + nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt); + nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; + nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; + nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; + nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt); + nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt); + nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt); + nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); + nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); + nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); + nextP[8][10] = P[8][10] + P[5][10]*dt; + nextP[8][11] = P[8][11] + P[5][11]*dt; + nextP[8][12] = P[8][12] + P[5][12]*dt; + nextP[8][13] = P[8][13] + P[5][13]*dt; + nextP[8][14] = P[8][14] + P[5][14]*dt; + nextP[8][15] = P[8][15] + P[5][15]*dt; + nextP[8][16] = P[8][16] + P[5][16]*dt; + nextP[8][17] = P[8][17] + P[5][17]*dt; + nextP[8][18] = P[8][18] + P[5][18]*dt; + nextP[8][19] = P[8][19] + P[5][19]*dt; + nextP[8][20] = P[8][20] + P[5][20]*dt; + nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt); + nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; + nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; + nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; + nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt); + nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt); + nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt); + nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); + nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); + nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); + nextP[9][10] = P[9][10] + P[6][10]*dt; + nextP[9][11] = P[9][11] + P[6][11]*dt; + nextP[9][12] = P[9][12] + P[6][12]*dt; + nextP[9][13] = P[9][13] + P[6][13]*dt; + nextP[9][14] = P[9][14] + P[6][14]*dt; + nextP[9][15] = P[9][15] + P[6][15]*dt; + nextP[9][16] = P[9][16] + P[6][16]*dt; + nextP[9][17] = P[9][17] + P[6][17]*dt; + nextP[9][18] = P[9][18] + P[6][18]*dt; + nextP[9][19] = P[9][19] + P[6][19]*dt; + nextP[9][20] = P[9][20] + P[6][20]*dt; + nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3]; + nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2; + nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2; + nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2; + nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2]; + nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0]; + nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1]; + nextP[10][7] = P[10][7] + P[10][4]*dt; + nextP[10][8] = P[10][8] + P[10][5]*dt; + nextP[10][9] = P[10][9] + P[10][6]*dt; + nextP[10][10] = P[10][10]; + nextP[10][11] = P[10][11]; + nextP[10][12] = P[10][12]; + nextP[10][13] = P[10][13]; + nextP[10][14] = P[10][14]; + nextP[10][15] = P[10][15]; + nextP[10][16] = P[10][16]; + nextP[10][17] = P[10][17]; + nextP[10][18] = P[10][18]; + nextP[10][19] = P[10][19]; + nextP[10][20] = P[10][20]; + nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3]; + nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2; + nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2; + nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2; + nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2]; + nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0]; + nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1]; + nextP[11][7] = P[11][7] + P[11][4]*dt; + nextP[11][8] = P[11][8] + P[11][5]*dt; + nextP[11][9] = P[11][9] + P[11][6]*dt; + nextP[11][10] = P[11][10]; + nextP[11][11] = P[11][11]; + nextP[11][12] = P[11][12]; + nextP[11][13] = P[11][13]; + nextP[11][14] = P[11][14]; + nextP[11][15] = P[11][15]; + nextP[11][16] = P[11][16]; + nextP[11][17] = P[11][17]; + nextP[11][18] = P[11][18]; + nextP[11][19] = P[11][19]; + nextP[11][20] = P[11][20]; + nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3]; + nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2; + nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2; + nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2; + nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2]; + nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0]; + nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1]; + nextP[12][7] = P[12][7] + P[12][4]*dt; + nextP[12][8] = P[12][8] + P[12][5]*dt; + nextP[12][9] = P[12][9] + P[12][6]*dt; + nextP[12][10] = P[12][10]; + nextP[12][11] = P[12][11]; + nextP[12][12] = P[12][12]; + nextP[12][13] = P[12][13]; + nextP[12][14] = P[12][14]; + nextP[12][15] = P[12][15]; + nextP[12][16] = P[12][16]; + nextP[12][17] = P[12][17]; + nextP[12][18] = P[12][18]; + nextP[12][19] = P[12][19]; + nextP[12][20] = P[12][20]; + nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3]; + nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2; + nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2; + nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2; + nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2]; + nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0]; + nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1]; + nextP[13][7] = P[13][7] + P[13][4]*dt; + nextP[13][8] = P[13][8] + P[13][5]*dt; + nextP[13][9] = P[13][9] + P[13][6]*dt; + nextP[13][10] = P[13][10]; + nextP[13][11] = P[13][11]; + nextP[13][12] = P[13][12]; + nextP[13][13] = P[13][13]; + nextP[13][14] = P[13][14]; + nextP[13][15] = P[13][15]; + nextP[13][16] = P[13][16]; + nextP[13][17] = P[13][17]; + nextP[13][18] = P[13][18]; + nextP[13][19] = P[13][19]; + nextP[13][20] = P[13][20]; + nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3]; + nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2; + nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2; + nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2; + nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2]; + nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0]; + nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1]; + nextP[14][7] = P[14][7] + P[14][4]*dt; + nextP[14][8] = P[14][8] + P[14][5]*dt; + nextP[14][9] = P[14][9] + P[14][6]*dt; + nextP[14][10] = P[14][10]; + nextP[14][11] = P[14][11]; + nextP[14][12] = P[14][12]; + nextP[14][13] = P[14][13]; + nextP[14][14] = P[14][14]; + nextP[14][15] = P[14][15]; + nextP[14][16] = P[14][16]; + nextP[14][17] = P[14][17]; + nextP[14][18] = P[14][18]; + nextP[14][19] = P[14][19]; + nextP[14][20] = P[14][20]; + nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3]; + nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2; + nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2; + nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2; + nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2]; + nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0]; + nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1]; + nextP[15][7] = P[15][7] + P[15][4]*dt; + nextP[15][8] = P[15][8] + P[15][5]*dt; + nextP[15][9] = P[15][9] + P[15][6]*dt; + nextP[15][10] = P[15][10]; + nextP[15][11] = P[15][11]; + nextP[15][12] = P[15][12]; + nextP[15][13] = P[15][13]; + nextP[15][14] = P[15][14]; + nextP[15][15] = P[15][15]; + nextP[15][16] = P[15][16]; + nextP[15][17] = P[15][17]; + nextP[15][18] = P[15][18]; + nextP[15][19] = P[15][19]; + nextP[15][20] = P[15][20]; + nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3]; + nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2; + nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2; + nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2; + nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2]; + nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0]; + nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1]; + nextP[16][7] = P[16][7] + P[16][4]*dt; + nextP[16][8] = P[16][8] + P[16][5]*dt; + nextP[16][9] = P[16][9] + P[16][6]*dt; + nextP[16][10] = P[16][10]; + nextP[16][11] = P[16][11]; + nextP[16][12] = P[16][12]; + nextP[16][13] = P[16][13]; + nextP[16][14] = P[16][14]; + nextP[16][15] = P[16][15]; + nextP[16][16] = P[16][16]; + nextP[16][17] = P[16][17]; + nextP[16][18] = P[16][18]; + nextP[16][19] = P[16][19]; + nextP[16][20] = P[16][20]; + nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3]; + nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2; + nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2; + nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2; + nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2]; + nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0]; + nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1]; + nextP[17][7] = P[17][7] + P[17][4]*dt; + nextP[17][8] = P[17][8] + P[17][5]*dt; + nextP[17][9] = P[17][9] + P[17][6]*dt; + nextP[17][10] = P[17][10]; + nextP[17][11] = P[17][11]; + nextP[17][12] = P[17][12]; + nextP[17][13] = P[17][13]; + nextP[17][14] = P[17][14]; + nextP[17][15] = P[17][15]; + nextP[17][16] = P[17][16]; + nextP[17][17] = P[17][17]; + nextP[17][18] = P[17][18]; + nextP[17][19] = P[17][19]; + nextP[17][20] = P[17][20]; + nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3]; + nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2; + nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2; + nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2; + nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2]; + nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0]; + nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1]; + nextP[18][7] = P[18][7] + P[18][4]*dt; + nextP[18][8] = P[18][8] + P[18][5]*dt; + nextP[18][9] = P[18][9] + P[18][6]*dt; + nextP[18][10] = P[18][10]; + nextP[18][11] = P[18][11]; + nextP[18][12] = P[18][12]; + nextP[18][13] = P[18][13]; + nextP[18][14] = P[18][14]; + nextP[18][15] = P[18][15]; + nextP[18][16] = P[18][16]; + nextP[18][17] = P[18][17]; + nextP[18][18] = P[18][18]; + nextP[18][19] = P[18][19]; + nextP[18][20] = P[18][20]; + nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3]; + nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2; + nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2; + nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2; + nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2]; + nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0]; + nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1]; + nextP[19][7] = P[19][7] + P[19][4]*dt; + nextP[19][8] = P[19][8] + P[19][5]*dt; + nextP[19][9] = P[19][9] + P[19][6]*dt; + nextP[19][10] = P[19][10]; + nextP[19][11] = P[19][11]; + nextP[19][12] = P[19][12]; + nextP[19][13] = P[19][13]; + nextP[19][14] = P[19][14]; + nextP[19][15] = P[19][15]; + nextP[19][16] = P[19][16]; + nextP[19][17] = P[19][17]; + nextP[19][18] = P[19][18]; + nextP[19][19] = P[19][19]; + nextP[19][20] = P[19][20]; + nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3]; + nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2; + nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2; + nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2; + nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2]; + nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0]; + nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1]; + nextP[20][7] = P[20][7] + P[20][4]*dt; + nextP[20][8] = P[20][8] + P[20][5]*dt; + nextP[20][9] = P[20][9] + P[20][6]*dt; + nextP[20][10] = P[20][10]; + nextP[20][11] = P[20][11]; + nextP[20][12] = P[20][12]; + nextP[20][13] = P[20][13]; + nextP[20][14] = P[20][14]; + nextP[20][15] = P[20][15]; + nextP[20][16] = P[20][16]; + nextP[20][17] = P[20][17]; + nextP[20][18] = P[20][18]; + nextP[20][19] = P[20][19]; + nextP[20][20] = P[20][20]; + + for (unsigned i = 0; i < n_states; i++) + { + nextP[i][i] = nextP[i][i] + processNoise[i]; + } + + // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by + // setting the coresponding covariance terms to zero. + if (onGround || !useCompass) + { + zeroRows(nextP,15,20); + zeroCols(nextP,15,20); + } + + // If on ground or not using airspeed sensing, inhibit wind velocity + // covariance growth. + if (onGround || !useAirspeed) + { + zeroRows(nextP,13,14); + zeroCols(nextP,13,14); + } + + // If the total position variance exceds 1E6 (1000m), then stop covariance + // growth by setting the predicted to the previous values + // This prevent an ill conditioned matrix from occurring for long periods + // without GPS + if ((P[7][7] + P[8][8]) > 1E6f) + { + for (uint8_t i=7; i<=8; i++) + { + for (unsigned j = 0; j < n_states; j++) + { + nextP[i][j] = P[i][j]; + nextP[j][i] = P[j][i]; + } + } + } + + if (onGround || staticMode) { + // copy the portion of the variances we want to + // propagate + for (unsigned i = 0; i < 14; i++) { + P[i][i] = nextP[i][i]; + + // force symmetry for observable states + // force zero for non-observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + if ((i > 12) || (j > 12)) { + P[i][j] = 0.0f; + } else { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + } + P[j][i] = P[i][j]; + } + } + } + + } else { + + // Copy covariance + for (unsigned i = 0; i < n_states; i++) { + P[i][i] = nextP[i][i]; + } + + // force symmetry for observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; + } + } + + } + + ConstrainVariances(); +} + +void AttPosEKF::FuseVelposNED() +{ + +// declare variables used by fault isolation logic + uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure + uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available + uint32_t hgtRetryTime = 5000; // height measurement retry time + uint32_t horizRetryTime; + +// declare variables used to check measurement errors + float velInnov[3] = {0.0f,0.0f,0.0f}; + float posInnov[2] = {0.0f,0.0f}; + float hgtInnov = 0.0f; + +// declare variables used to control access to arrays + bool fuseData[6] = {false,false,false,false,false,false}; + uint8_t stateIndex; + uint8_t obsIndex; + uint8_t indexLimit; + +// declare variables used by state and covariance update calculations + float velErr; + float posErr; + float R_OBS[6]; + float observation[6]; + float SK; + float quatMag; + +// Perform sequential fusion of GPS measurements. This assumes that the +// errors in the different velocity and position components are +// uncorrelated which is not true, however in the absence of covariance +// data from the GPS receiver it is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion + if (fuseVelData || fusePosData || fuseHgtData) + { + // set the GPS data timeout depending on whether airspeed data is present + if (useAirspeed) horizRetryTime = gpsRetryTime; + else horizRetryTime = gpsRetryTimeNoTAS; + + // Form the observation vector + for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; + for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; + observation[5] = -(hgtMea); + + // Estimate the GPS Velocity, GPS horiz position and height measurement variances. + velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring + posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring + R_OBS[0] = 0.04f + sq(velErr); + R_OBS[1] = R_OBS[0]; + R_OBS[2] = 0.08f + sq(velErr); + R_OBS[3] = R_OBS[2]; + R_OBS[4] = 4.0f + sq(posErr); + R_OBS[5] = 4.0f; + + // Set innovation variances to zero default + for (uint8_t i = 0; i<=5; i++) + { + varInnovVelPos[i] = 0.0f; + } + // calculate innovations and check GPS data validity using an innovation consistency check + if (fuseVelData) + { + // test velocity measurements + uint8_t imax = 2; + if (fusionModeGPS == 1) imax = 1; + for (uint8_t i = 0; i<=imax; i++) + { + velInnov[i] = statesAtVelTime[i+4] - velNED[i]; + stateIndex = 4 + i; + varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; + } + // apply a 5-sigma threshold + current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); + current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; + if (current_ekf_state.velHealth || current_ekf_state.velTimeout) + { + current_ekf_state.velHealth = true; + current_ekf_state.velFailTime = millis(); + } + else + { + current_ekf_state.velHealth = false; + } + } + if (fusePosData) + { + // test horizontal position measurements + posInnov[0] = statesAtPosTime[7] - posNE[0]; + posInnov[1] = statesAtPosTime[8] - posNE[1]; + varInnovVelPos[3] = P[7][7] + R_OBS[3]; + varInnovVelPos[4] = P[8][8] + R_OBS[4]; + // apply a 10-sigma threshold + current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); + current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; + if (current_ekf_state.posHealth || current_ekf_state.posTimeout) + { + current_ekf_state.posHealth = true; + current_ekf_state.posFailTime = millis(); + } + else + { + current_ekf_state.posHealth = false; + } + } + // test height measurements + if (fuseHgtData) + { + hgtInnov = statesAtHgtTime[9] + hgtMea; + varInnovVelPos[5] = P[9][9] + R_OBS[5]; + // apply a 10-sigma threshold + current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; + current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) + { + current_ekf_state.hgtHealth = true; + current_ekf_state.hgtFailTime = millis(); + } + else + { + current_ekf_state.hgtHealth = false; + } + } + // Set range for sequential fusion of velocity and position measurements depending + // on which data is available and its health + if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + fuseData[2] = true; + } + if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + } + if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) + { + fuseData[3] = true; + fuseData[4] = true; + } + if (fuseHgtData && current_ekf_state.hgtHealth) + { + fuseData[5] = true; + } + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 20; + } + else + { + indexLimit = 12; + } + // Fuse measurements sequentially + for (obsIndex=0; obsIndex<=5; obsIndex++) + { + if (fuseData[obsIndex]) + { + stateIndex = 4 + obsIndex; + // Calculate the measurement innovation, using states from a + // different time coordinate if fusing height data + if (obsIndex >= 0 && obsIndex <= 2) + { + innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 3 || obsIndex == 4) + { + innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 5) + { + innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; + } + // Calculate the Kalman Gain + // Calculate innovation variances - also used for data logging + varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; + SK = 1.0/varInnovVelPos[obsIndex]; + for (uint8_t i= 0; i<=indexLimit; i++) + { + Kfusion[i] = P[i][stateIndex]*SK; + } + // Calculate state corrections and re-normalise the quaternions + for (uint8_t i = 0; i<=indexLimit; i++) + { + states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; + } + quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) // divide by 0 protection + { + for (uint8_t i = 0; i<=3; i++) + { + states[i] = states[i] / quatMag; + } + } + // Update the covariance - take advantage of direct observation of a + // single state at index = stateIndex to reduce computations + // Optimised implementation of standard equation P = (I - K*H)*P; + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + KHP[i][j] = Kfusion[i] * P[stateIndex][j]; + } + } + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + } + + ForceSymmetry(); + ConstrainVariances(); + + //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); +} + +void AttPosEKF::FuseMagnetometer() +{ + uint8_t obsIndex; + uint8_t indexLimit; + float DCM[3][3] = + { + {1.0f,0.0f,0.0f} , + {0.0f,1.0f,0.0f} , + {0.0f,0.0f,1.0f} + }; + float MagPred[3] = {0.0f,0.0f,0.0f}; + float SK_MX[6]; + float SK_MY[5]; + float SK_MZ[6]; + float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; + +// Perform sequential fusion of Magnetometer measurements. +// This assumes that the errors in the different components are +// uncorrelated which is not true, however in the absence of covariance +// data fit is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion + if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) + { + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 20; + } + else + { + indexLimit = 12; + } + + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float magN = 0.4f; + static float magE = 0.0f; + static float magD = 0.3f; + + static float R_MAG = 0.0025f; + + float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; + + // Sequential fusion of XYZ components to spread processing load across + // three prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseMagData) + { + static float magXbias = 0.0f; + static float magYbias = 0.0f; + static float magZbias = 0.0f; + + // Copy required states to local variable names + q0 = statesAtMagMeasTime[0]; + q1 = statesAtMagMeasTime[1]; + q2 = statesAtMagMeasTime[2]; + q3 = statesAtMagMeasTime[3]; + magN = statesAtMagMeasTime[15]; + magE = statesAtMagMeasTime[16]; + magD = statesAtMagMeasTime[17]; + magXbias = statesAtMagMeasTime[18]; + magYbias = statesAtMagMeasTime[19]; + magZbias = statesAtMagMeasTime[20]; + + // rotate predicted earth components into body axes and calculate + // predicted measurments + DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; + DCM[0][1] = 2*(q1*q2 + q0*q3); + DCM[0][2] = 2*(q1*q3-q0*q2); + DCM[1][0] = 2*(q1*q2 - q0*q3); + DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3; + DCM[1][2] = 2*(q2*q3 + q0*q1); + DCM[2][0] = 2*(q1*q3 + q0*q2); + DCM[2][1] = 2*(q2*q3 - q0*q1); + DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3; + MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias; + MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; + MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; + + // scale magnetometer observation error with total angular rate + R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU); + + // Calculate observation jacobians + SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; + SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SH_MAG[3] = sq(q3); + SH_MAG[4] = sq(q2); + SH_MAG[5] = sq(q1); + SH_MAG[6] = sq(q0); + SH_MAG[7] = 2*magN*q0; + SH_MAG[8] = 2*magE*q3; + + for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[1] = SH_MAG[0]; + H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; + H_MAG[3] = SH_MAG[2]; + H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[16] = 2*q0*q3 + 2*q1*q2; + H_MAG[17] = 2*q1*q3 - 2*q0*q2; + H_MAG[18] = 1.0f; + + // Calculate Kalman gain + SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; + SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MX[4] = 2*q0*q2 - 2*q1*q3; + SK_MX[5] = 2*q0*q3 + 2*q1*q2; + Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]); + Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]); + Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]); + Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]); + Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]); + Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]); + Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]); + Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]); + Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]); + Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]); + Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]); + Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]); + Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]); + Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]); + Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]); + Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]); + varInnovMag[0] = 1.0f/SK_MX[0]; + innovMag[0] = MagPred[0] - magData.x; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (unsigned int i=0; i 5Sigma + if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) + { + // correct the state vector + for (uint8_t j= 0; j<=indexLimit; j++) + { + states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=3; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f; + if (!onGround) + { + for (uint8_t j = 15; j<=20; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + } + else + { + for (uint8_t j = 15; j<=20; j++) + { + KH[i][j] = 0.0f; + } + } + } + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=indexLimit; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k<=3; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + if (!onGround) + { + for (uint8_t k = 15; k<=20; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + } + } + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; + + ForceSymmetry(); + ConstrainVariances(); +} + +void AttPosEKF::FuseAirspeed() +{ + float vn; + float ve; + float vd; + float vwn; + float vwe; + const float R_TAS = 2.0f; + float SH_TAS[3]; + float Kfusion[21]; + float VtasPred; + + // Copy required states to local variable names + vn = statesAtVtasMeasTime[4]; + ve = statesAtVtasMeasTime[5]; + vd = statesAtVtasMeasTime[6]; + vwn = statesAtVtasMeasTime[13]; + vwe = statesAtVtasMeasTime[14]; + + // Need to check that it is flying before fusing airspeed data + // Calculate the predicted airspeed + VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); + // Perform fusion of True Airspeed measurement + if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) + { + // Calculate observation jacobians + SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; + SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; + + float H_TAS[21]; + for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f; + H_TAS[4] = SH_TAS[2]; + H_TAS[5] = SH_TAS[1]; + H_TAS[6] = vd*SH_TAS[0]; + H_TAS[13] = -SH_TAS[2]; + H_TAS[14] = -SH_TAS[1]; + + // Calculate Kalman gains + float SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); + Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); + Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); + Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); + Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); + Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); + Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); + Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); + Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); + Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); + Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); + Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); + Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); + Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); + Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); + Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); + varInnovVtas = 1.0f/SK_TAS; + + // Calculate the measurement innovation + innovVtas = VtasPred - VtasMeas; + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovVtas*innovVtas*SK_TAS) < 25.0) + { + // correct the state vector + for (uint8_t j=0; j<=20; j++) + { + states[j] = states[j] - Kfusion[j] * innovVtas; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in H to reduce the + // number of operations + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0; + for (uint8_t j = 4; j<=6; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0; + for (uint8_t j = 13; j<=14; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0; + } + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=20; j++) + { + KHP[i][j] = 0.0; + for (uint8_t k = 4; k<=6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + for (uint8_t k = 13; k<=14; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=20; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + + ForceSymmetry(); + ConstrainVariances(); +} + +void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (row=first; row<=last; row++) + { + for (col=0; col GPS_FIX_2D); + } +} + +void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) +{ + //Define Earth rotation vector in the NED navigation frame + omega.x = earthRate*cosf(latitude); + omega.y = 0.0f; + omega.z = -earthRate*sinf(latitude); +} + +void AttPosEKF::CovarianceInit() +{ + // Calculate the initial covariance matrix P + P[0][0] = 0.25f * sq(1.0f*deg2rad); + P[1][1] = 0.25f * sq(1.0f*deg2rad); + P[2][2] = 0.25f * sq(1.0f*deg2rad); + P[3][3] = 0.25f * sq(10.0f*deg2rad); + P[4][4] = sq(0.7); + P[5][5] = P[4][4]; + P[6][6] = sq(0.7); + P[7][7] = sq(15.0); + P[8][8] = P[7][7]; + P[9][9] = sq(5.0); + P[10][10] = sq(0.1*deg2rad*dtIMU); + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; + P[13][13] = sq(8.0f); + P[14][4] = P[13][13]; + P[15][15] = sq(0.02f); + P[16][16] = P[15][15]; + P[17][17] = P[15][15]; + P[18][18] = sq(0.02f); + P[19][19] = P[18][18]; + P[20][20] = P[18][18]; +} + +float AttPosEKF::ConstrainFloat(float val, float min, float max) +{ + return (val > max) ? max : ((val < min) ? min : val); +} + +void AttPosEKF::ConstrainVariances() +{ + if (!numericalProtection) { + return; + } + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + + // Constrain quaternion variances + for (unsigned i = 0; i < 4; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Constrain velocitie variances + for (unsigned i = 4; i < 7; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Constrain position variances + for (unsigned i = 7; i < 10; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); + } + + // Angle bias variances + for (unsigned i = 10; i < 13; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU)); + } + + // Wind velocity variances + for (unsigned i = 13; i < 15; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Earth magnetic field variances + for (unsigned i = 15; i < 18; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Body magnetic field variances + for (unsigned i = 18; i < 21; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + +} + +void AttPosEKF::ConstrainStates() +{ + if (!numericalProtection) { + return; + } + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + + + // Constrain quaternion + for (unsigned i = 0; i < 4; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Constrain velocities to what GPS can do for us + for (unsigned i = 4; i < 7; i++) { + states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); + } + + // Constrain position to a reasonable vehicle range (in meters) + for (unsigned i = 7; i < 9; i++) { + states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); + } + + // Constrain altitude + states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); + + // Angle bias limit - set to 8 degrees / sec + for (unsigned i = 10; i < 13; i++) { + states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); + } + + // Wind velocity limits - assume 120 m/s max velocity + for (unsigned i = 13; i < 15; i++) { + states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); + } + + // Earth magnetic field limits (in Gauss) + for (unsigned i = 15; i < 18; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Body magnetic field variances (in Gauss). + // the max offset should be in this range. + for (unsigned i = 18; i < 21; i++) { + states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); + } + +} + +void AttPosEKF::ForceSymmetry() +{ + if (!numericalProtection) { + return; + } + + // Force symmetry on the covariance matrix to prevent ill-conditioning + // of the matrix which would cause the filter to blow-up + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (P[i][j] + P[j][i]); + P[j][i] = P[i][j]; + } + } +} + +bool AttPosEKF::FilterHealthy() +{ + if (!statesInitialised) { + return false; + } + + // XXX Check state vector for NaNs and ill-conditioning + + // Check if any of the major inputs timed out + if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { + return false; + } + + // Nothing fired, return ok. + return true; +} + +/** + * Reset the filter position states + * + * This resets the position to the last GPS measurement + * or to zero in case of static position. + */ +void AttPosEKF::ResetPosition(void) +{ + if (staticMode) { + states[7] = 0; + states[8] = 0; + } else if (GPSstatus >= GPS_FIX_3D) { + + // reset the states from the GPS measurements + states[7] = posNE[0]; + states[8] = posNE[1]; + } +} + +/** + * Reset the height state. + * + * This resets the height state with the last altitude measurements + */ +void AttPosEKF::ResetHeight(void) +{ + // write to the state vector + states[9] = -hgtMea; +} + +/** + * Reset the velocity state. + */ +void AttPosEKF::ResetVelocity(void) +{ + if (staticMode) { + states[4] = 0.0f; + states[5] = 0.0f; + states[6] = 0.0f; + } else if (GPSstatus >= GPS_FIX_3D) { + + states[4] = velNED[0]; // north velocity from last reading + states[5] = velNED[1]; // east velocity from last reading + states[6] = velNED[2]; // down velocity from last reading + } +} + + +void AttPosEKF::FillErrorReport(struct ekf_status_report *err) +{ + for (int i = 0; i < n_states; i++) + { + err->states[i] = states[i]; + } + + err->velHealth = current_ekf_state.velHealth; + err->posHealth = current_ekf_state.posHealth; + err->hgtHealth = current_ekf_state.hgtHealth; + err->velTimeout = current_ekf_state.velTimeout; + err->posTimeout = current_ekf_state.posTimeout; + err->hgtTimeout = current_ekf_state.hgtTimeout; +} + +bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { + bool err = false; + + // check all states and covariance matrices + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + if (!isfinite(KH[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // intermediate result used for covariance updates + if (!isfinite(KHP[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // intermediate result used for covariance updates + if (!isfinite(P[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // covariance matrix + } + + if (!isfinite(Kfusion[i])) { + + err_report->kalmanGainsNaN = true; + err = true; + } // Kalman gains + + if (!isfinite(states[i])) { + + err_report->statesNaN = true; + err = true; + } // state matrix + } + + if (err) { + FillErrorReport(err_report); + } + + return err; + +} + +/** + * Check the filter inputs and bound its operational state + * + * This check will reset the filter states if required + * due to a failure of consistency or timeout checks. + * it should be run after the measurement data has been + * updated, but before any of the fusion steps are + * executed. + */ +int AttPosEKF::CheckAndBound() +{ + + // Store the old filter state + bool currStaticMode = staticMode; + + // Reset the filter if the states went NaN + if (StatesNaN(&last_ekf_error)) { + + InitializeDynamic(velNED, magDeclination); + + return 1; + } + + // Reset the filter if the IMU data is too old + if (dtIMU > 0.2f) { + + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + // that's all we can do here, return + return 2; + } + + // Check if we're on ground - this also sets static mode. + OnGroundCheck(); + + // Check if we switched between states + if (currStaticMode != staticMode) { + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + return 3; + } + + return 0; +} + +void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) +{ + float initialRoll, initialPitch; + float cosRoll, sinRoll, cosPitch, sinPitch; + float magX, magY; + float initialHdg, cosHeading, sinHeading; + + initialRoll = atan2(-ay, -az); + initialPitch = atan2(ax, -az); + + cosRoll = cosf(initialRoll); + sinRoll = sinf(initialRoll); + cosPitch = cosf(initialPitch); + sinPitch = sinf(initialPitch); + + magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; + + magY = my * cosRoll - mz * sinRoll; + + initialHdg = atan2f(-magY, magX); + /* true heading is the mag heading minus declination */ + initialHdg += declination; + + cosRoll = cosf(initialRoll * 0.5f); + sinRoll = sinf(initialRoll * 0.5f); + + cosPitch = cosf(initialPitch * 0.5f); + sinPitch = sinf(initialPitch * 0.5f); + + cosHeading = cosf(initialHdg * 0.5f); + sinHeading = sinf(initialHdg * 0.5f); + + initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; + initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; + initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; + initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; +} + +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) +{ + + // Clear the init flag + statesInitialised = false; + + magDeclination = declination; + + ZeroVariables(); + + // Calculate initial filter quaternion states from raw measurements + float initQuat[4]; + Vector3f initMagXYZ; + initMagXYZ = magData - magBias; + AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, magDeclination, initQuat); + + // Calculate initial Tbn matrix and rotate Mag measurements into NED + // to set initial NED magnetic field states + Mat3f DCM; + quat2Tbn(DCM, initQuat); + Vector3f initMagNED; + initMagXYZ = magData - magBias; + initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; + initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; + initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; + + + + // write to state vector + for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions + for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities + for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel + states[15] = initMagNED.x; // Magnetic Field North + states[16] = initMagNED.y; // Magnetic Field East + states[17] = initMagNED.z; // Magnetic Field Down + states[18] = magBias.x; // Magnetic Field Bias X + states[19] = magBias.y; // Magnetic Field Bias Y + states[20] = magBias.z; // Magnetic Field Bias Z + + statesInitialised = true; + + // initialise the covariance matrix + CovarianceInit(); + + //Define Earth rotation vector in the NED navigation frame + calcEarthRateNED(earthRateNED, latRef); + + //Initialise summed variables used by covariance prediction + summedDelAng.x = 0.0f; + summedDelAng.y = 0.0f; + summedDelAng.z = 0.0f; + summedDelVel.x = 0.0f; + summedDelVel.y = 0.0f; + summedDelVel.z = 0.0f; +} + +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) +{ + //store initial lat,long and height + latRef = referenceLat; + lonRef = referenceLon; + hgtRef = referenceHgt; + + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + + InitializeDynamic(initvelNED, declination); +} + +void AttPosEKF::ZeroVariables() +{ + // Do the data structure init + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + KH[i][j] = 0.0f; // intermediate result used for covariance updates + KHP[i][j] = 0.0f; // intermediate result used for covariance updates + P[i][j] = 0.0f; // covariance matrix + } + + Kfusion[i] = 0.0f; // Kalman gains + states[i] = 0.0f; // state matrix + } + + for (unsigned i = 0; i < data_buffer_size; i++) { + + for (unsigned j = 0; j < n_states; j++) { + storedStates[j][i] = 0.0f; + } + + statetimeStamp[i] = 0; + } + + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); +} + +void AttPosEKF::GetFilterState(struct ekf_status_report *state) +{ + memcpy(state, ¤t_ekf_state, sizeof(state)); +} + +void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) +{ + memcpy(last_error, &last_ekf_error, sizeof(last_error)); +} diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.h b/src/modules/ekf_att_pos_estimator/estimator_21states.h new file mode 100644 index 000000000..a19ff1995 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_21states.h @@ -0,0 +1,247 @@ +#pragma once + +#include "estimator_utilities.h" + +class AttPosEKF { + +public: + + AttPosEKF(); + ~AttPosEKF(); + + /* ############################################## + * + * M A I N F I L T E R P A R A M E T E R S + * + * ########################################### */ + + /* + * parameters are defined here and initialised in + * the InitialiseParameters() (which is just 20 lines down) + */ + + float covTimeStepMax; // maximum time allowed between covariance predictions + float covDelAngMax; // maximum delta angle between covariance predictions + float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + + float yawVarScale; + float windVelSigma; + float dAngBiasSigma; + float dVelBiasSigma; + float magEarthSigma; + float magBodySigma; + float gndHgtSigma; + + float vneSigma; + float vdSigma; + float posNeSigma; + float posDSigma; + float magMeasurementSigma; + float airspeedMeasurementSigma; + + float gyroProcessNoise; + float accelProcessNoise; + + float EAS2TAS; // ratio f true to equivalent airspeed + + void InitialiseParameters() + { + covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions + covDelAngMax = 0.02f; // maximum delta angle between covariance predictions + rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + EAS2TAS = 1.0f; + + yawVarScale = 1.0f; + windVelSigma = 0.1f; + dAngBiasSigma = 5.0e-7f; + dVelBiasSigma = 1e-4f; + magEarthSigma = 3.0e-4f; + magBodySigma = 3.0e-4f; + gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma + + vneSigma = 0.2f; + vdSigma = 0.3f; + posNeSigma = 2.0f; + posDSigma = 2.0f; + + magMeasurementSigma = 0.05; + airspeedMeasurementSigma = 1.4f; + gyroProcessNoise = 1.4544411e-2f; + accelProcessNoise = 0.5f; + } + + // Global variables + float KH[n_states][n_states]; // intermediate result used for covariance updates + float KHP[n_states][n_states]; // intermediate result used for covariance updates + float P[n_states][n_states]; // covariance matrix + float Kfusion[n_states]; // Kalman gains + float states[n_states]; // state matrix + float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps + uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored + + float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements + float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements + float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement + float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time + float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time + + Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) + Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) + Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) + Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) + float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) + Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) + Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) + Vector3f dVelIMU; + Vector3f dAngIMU; + float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) + uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity + float innovVelPos[6]; // innovation output + float varInnovVelPos[6]; // innovation variance output + + float velNED[3]; // North, East, Down velocity obs (m/s) + float posNE[2]; // North, East position obs (m) + float hgtMea; // measured height (m) + float posNED[3]; // North, East Down position (m) + + float innovMag[3]; // innovation output + float varInnovMag[3]; // innovation variance output + Vector3f magData; // magnetometer flux radings in X,Y,Z body axes + float innovVtas; // innovation output + float varInnovVtas; // innovation variance output + float VtasMeas; // true airspeed measurement (m/s) + float magDeclination; + float latRef; // WGS-84 latitude of reference point (rad) + float lonRef; // WGS-84 longitude of reference point (rad) + float hgtRef; // WGS-84 height of reference point (m) + Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes + uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction + + // GPS input data variables + float gpsCourse; + float gpsVelD; + float gpsLat; + float gpsLon; + float gpsHgt; + uint8_t GPSstatus; + + // Baro input + float baroHgt; + + bool statesInitialised; + + bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused + bool fusePosData; // this boolean causes the posNE and velNED obs to be fused + bool fuseHgtData; // this boolean causes the hgtMea obs to be fused + bool fuseMagData; // boolean true when magnetometer data is to be fused + bool fuseVtasData; // boolean true when airspeed data is to be fused + + bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) + bool staticMode; ///< boolean true if no position feedback is fused + bool useAirspeed; ///< boolean true if airspeed data is being used + bool useCompass; ///< boolean true if magnetometer data is being used + + struct ekf_status_report current_ekf_state; + struct ekf_status_report last_ekf_error; + + bool numericalProtection; + + unsigned storeIndex; + + +void UpdateStrapdownEquationsNED(); + +void CovariancePrediction(float dt); + +void FuseVelposNED(); + +void FuseMagnetometer(); + +void FuseAirspeed(); + +void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); + +void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); + +void quatNorm(float (&quatOut)[4], const float quatIn[4]); + +// store staes along with system time stamp in msces +void StoreStates(uint64_t timestamp_ms); + +/** + * Recall the state vector. + * + * Recalls the vector stored at closest time to the one specified by msec + * + * @return zero on success, integer indicating the number of invalid states on failure. + * Does only copy valid states, if the statesForFusion vector was initialized + * correctly by the caller, the result can be safely used, but is a mixture + * time-wise where valid states were updated and invalid remained at the old + * value. + */ +int RecallStates(float statesForFusion[n_states], uint64_t msec); + +void ResetStoredStates(); + +void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); + +void calcEarthRateNED(Vector3f &omega, float latitude); + +static void eul2quat(float (&quat)[4], const float (&eul)[3]); + +static void quat2eul(float (&eul)[3], const float (&quat)[4]); + +static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); + +static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); + +static float sq(float valIn); + +void OnGroundCheck(); + +void CovarianceInit(); + +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); + +float ConstrainFloat(float val, float min, float max); + +void ConstrainVariances(); + +void ConstrainStates(); + +void ForceSymmetry(); + +int CheckAndBound(); + +void ResetPosition(); + +void ResetVelocity(); + +void ZeroVariables(); + +void GetFilterState(struct ekf_status_report *state); + +void GetLastErrorState(struct ekf_status_report *last_error); + +bool StatesNaN(struct ekf_status_report *err_report); +void FillErrorReport(struct ekf_status_report *err); + +void InitializeDynamic(float (&initvelNED)[3], float declination); + +protected: + +bool FilterHealthy(); + +void ResetHeight(void); + +void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); + +}; + +uint32_t millis(); + diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp new file mode 100644 index 000000000..b4767a0d3 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -0,0 +1,139 @@ + +#include "estimator_utilities.h" + +// Define EKF_DEBUG here to enable the debug print calls +// if the macro is not set, these will be completely +// optimized out by the compiler. +//#define EKF_DEBUG + +#ifdef EKF_DEBUG +#include + +static void +ekf_debug_print(const char *fmt, va_list args) +{ + fprintf(stderr, "%s: ", "[ekf]"); + vfprintf(stderr, fmt, args); + + fprintf(stderr, "\n"); +} + +void +ekf_debug(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + ekf_debug_print(fmt, args); +} + +#else + +void ekf_debug(const char *fmt, ...) { while(0){} } +#endif + +float Vector3f::length(void) const +{ + return sqrt(x*x + y*y + z*z); +} + +void Vector3f::zero(void) +{ + x = 0.0f; + y = 0.0f; + z = 0.0f; +} + +Mat3f::Mat3f() { + identity(); +} + +void Mat3f::identity() { + x.x = 1.0f; + x.y = 0.0f; + x.z = 0.0f; + + y.x = 0.0f; + y.y = 1.0f; + y.z = 0.0f; + + z.x = 0.0f; + z.y = 0.0f; + z.z = 1.0f; +} + +Mat3f Mat3f::transpose(void) const +{ + Mat3f ret = *this; + swap_var(ret.x.y, ret.y.x); + swap_var(ret.x.z, ret.z.x); + swap_var(ret.y.z, ret.z.y); + return ret; +} + +// overload + operator to provide a vector addition +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x + vecIn2.x; + vecOut.y = vecIn1.y + vecIn2.y; + vecOut.z = vecIn1.z + vecIn2.z; + return vecOut; +} + +// overload - operator to provide a vector subtraction +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x - vecIn2.x; + vecOut.y = vecIn1.y - vecIn2.y; + vecOut.z = vecIn1.z - vecIn2.z; + return vecOut; +} + +// overload * operator to provide a matrix vector product +Vector3f operator*( Mat3f matIn, Vector3f vecIn) +{ + Vector3f vecOut; + vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; + vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; + vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; + return vecOut; +} + +// overload % operator to provide a vector cross product +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y; + vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z; + vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(Vector3f vecIn1, float sclIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(float sclIn1, Vector3f vecIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +void swap_var(float &d1, float &d2) +{ + float tmp = d1; + d1 = d2; + d2 = tmp; +} diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h new file mode 100644 index 000000000..7dddee468 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -0,0 +1,79 @@ +#include +#include + +#pragma once + +#define GRAVITY_MSS 9.80665f +#define deg2rad 0.017453292f +#define rad2deg 57.295780f +#define pi 3.141592657f +#define earthRate 0.000072921f +#define earthRadius 6378145.0f +#define earthRadiusInv 1.5678540e-7f + +class Vector3f +{ +private: +public: + float x; + float y; + float z; + + float length(void) const; + void zero(void); +}; + +class Mat3f +{ +private: +public: + Vector3f x; + Vector3f y; + Vector3f z; + + Mat3f(); + + void identity(); + Mat3f transpose(void) const; +}; + +Vector3f operator*(float sclIn1, Vector3f vecIn1); +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*( Mat3f matIn, Vector3f vecIn); +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*(Vector3f vecIn1, float sclIn1); + +void swap_var(float &d1, float &d2); + +const unsigned int n_states = 21; +const unsigned int data_buffer_size = 50; + +enum GPS_FIX { + GPS_FIX_NOFIX = 0, + GPS_FIX_2D = 2, + GPS_FIX_3D = 3 +}; + +struct ekf_status_report { + bool velHealth; + bool posHealth; + bool hgtHealth; + bool velTimeout; + bool posTimeout; + bool hgtTimeout; + uint32_t velFailTime; + uint32_t posFailTime; + uint32_t hgtFailTime; + float states[n_states]; + bool angNaN; + bool summedDelVelNaN; + bool KHNaN; + bool KHPNaN; + bool PNaN; + bool covarianceNaN; + bool kalmanGainsNaN; + bool statesNaN; +}; + +void ekf_debug(const char *fmt, ...); \ No newline at end of file diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 6fefec2c2..0058b3fd1 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -39,4 +39,5 @@ MODULE_COMMAND = ekf_att_pos_estimator SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ - estimator.cpp + estimator_21states.cpp \ + estimator_utilities.cpp -- cgit v1.2.3 From f4075b5623bb9c92dd46e92b97bc363a91498ff6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 16:06:18 +0200 Subject: Switching back to 23 states, fixed mag update logic --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 15 +++++++++++---- src/modules/ekf_att_pos_estimator/estimator_21states.cpp | 6 +++--- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 11 +++-------- src/modules/ekf_att_pos_estimator/estimator_23states.h | 2 +- src/modules/ekf_att_pos_estimator/estimator_utilities.h | 2 +- src/modules/ekf_att_pos_estimator/module.mk | 2 +- 6 files changed, 20 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 86f7eb99f..9cff9fab0 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -83,7 +83,7 @@ #include #include -#include "estimator_21states.h" +#include "estimator_23states.h" @@ -451,6 +451,8 @@ FixedwingEstimator::~FixedwingEstimator() } while (_estimator_task != -1); } + delete _ekf; + estimator::g_estimator = nullptr; } @@ -564,7 +566,7 @@ FixedwingEstimator::task_main() #else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_sensor_combined_sub, 4); + orb_set_interval(_sensor_combined_sub, 9); #endif /* sets also parameters in the EKF object */ @@ -809,6 +811,8 @@ FixedwingEstimator::task_main() #endif + //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); + bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); @@ -1247,12 +1251,15 @@ FixedwingEstimator::task_main() _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + _ekf->magstate.obsIndex = 0; + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + } else { _ekf->fuseMagData = false; } - if (_ekf->statesInitialised) _ekf->FuseMagnetometer(); - // Fuse Airspeed Measurements if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { _ekf->fuseVtasData = true; diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp index 5a60ac1c0..0f5a17a33 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp @@ -61,8 +61,8 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // Apply corrections for earths rotation rate and coning errors // * and + operators have been overloaded - correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); - + correctedDelAng = dAngIMU;//correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + correctedDelAng.z = 0; // Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); if (rotationMag < 1e-12f) @@ -151,7 +151,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; // Constrain states (to protect against filter divergence) - ConstrainStates(); + //ConstrainStates(); } void AttPosEKF::CovariancePrediction(float dt) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index ca9db9685..89e6d3948 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1,4 +1,4 @@ -#include "estimator.h" +#include "estimator_23states.h" #include #include @@ -1140,7 +1140,7 @@ void AttPosEKF::FuseMagnetometer() // data fit is the only assumption we can make // so we might as well take advantage of the computational efficiencies // associated with sequential fusion - if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) + if (useCompass && fuseMagData && (obsIndex < 3)) { // Limit range of states modified when on ground if(!onGround) @@ -1156,7 +1156,7 @@ void AttPosEKF::FuseMagnetometer() // three prediction time steps. // Calculate observation jacobians and Kalman gains - if (fuseMagData) + if (obsIndex == 0) { // Copy required states to local variable names q0 = statesAtMagMeasTime[0]; @@ -1251,11 +1251,6 @@ void AttPosEKF::FuseMagnetometer() Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - fuseMagData = false; } else if (obsIndex == 1) // we are now fusing the Y measurement { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 6d3b89108..851e46371 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -229,7 +229,7 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]); static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); +void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 7dddee468..714dfe623 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -46,7 +46,7 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1); void swap_var(float &d1, float &d2); -const unsigned int n_states = 21; +const unsigned int n_states = 23; const unsigned int data_buffer_size = 50; enum GPS_FIX { diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 0058b3fd1..dc5220bf0 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -39,5 +39,5 @@ MODULE_COMMAND = ekf_att_pos_estimator SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ - estimator_21states.cpp \ + estimator_23states.cpp \ estimator_utilities.cpp -- cgit v1.2.3 From 13b6dffb2e719219ffccccd6681d36e695165aa5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 7 Jun 2014 17:18:58 +0200 Subject: navigator: don't reset descend WP in RTL --- src/modules/navigator/rtl.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index dde35d5b6..8888c6b62 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -90,8 +90,7 @@ RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet) } if ((_rtl_state == RTL_STATE_CLIMB - || _rtl_state == RTL_STATE_RETURN - || _rtl_state == RTL_STATE_DESCEND) + || _rtl_state == RTL_STATE_RETURN) && is_mission_item_reached()) { advance_rtl(); set_rtl_item(pos_sp_triplet); -- cgit v1.2.3 From 2a79a9a4e4ebae2e85dab64ae98ff2a17e421ea2 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Thu, 5 Jun 2014 04:03:40 -0400 Subject: Close fds when not needed File descriptors get closed when not needed by offboard mode to allow position and attitude controllers to advertise and publish. --- src/modules/mavlink/mavlink_receiver.cpp | 39 ++++++++++++++++++++++ src/modules/mc_att_control/mc_att_control_main.cpp | 8 ++++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 ++++- 3 files changed, 53 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2157ae00b..3b3423a4b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -422,6 +422,17 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message loc_pos_sp.yaw = offboard_control_sp.p3; loc_pos_sp.z = offboard_control_sp.p4; + /* Close fds to allow position controller to use attitude controller */ + if (_att_sp_pub > 0) { + close(_att_sp_pub); + _att_sp_pub = -1; + } + + if (_rates_sp_pub > 0) { + close(_rates_sp_pub); + _rates_sp_pub = -1; + } + if (_local_pos_sp_pub < 0) { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub); @@ -444,6 +455,12 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message att_sp.timestamp = hrt_absolute_time(); + /* Close fd to allow attitude controller to publish its own rates sp*/ + if (_rates_sp_pub > 0) { + close(_rates_sp_pub); + _rates_sp_pub = -1; + } + if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); @@ -993,6 +1010,28 @@ MavlinkReceiver::receive_thread(void *arg) } } } + /* Close unused fds when not in offboard mode anymore */ + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + if (!_control_mode.flag_control_offboard_enabled) { + if (_local_pos_sp_pub > 0) { + close(_local_pos_sp_pub); + _local_pos_sp_pub = -1; + } + + if (_att_sp_pub > 0) { + close(_att_sp_pub); + _att_sp_pub = -1; + } + + if (_rates_sp_pub > 0) { + close(_rates_sp_pub); + _rates_sp_pub = -1; + } + } + } } return NULL; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 20e016da3..26c0c386f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -584,9 +584,15 @@ MulticopterAttitudeControl::control_attitude(float dt) if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); - } else { + } else if (!_v_control_mode.flag_control_offboard_enabled) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); } + + /* Close fd to let offboard att sp be advertised in mavlink receiver*/ + if (_v_control_mode.flag_control_offboard_enabled && _att_sp_pub > 0) { + close(_att_sp_pub); + _att_sp_pub = -1; + } } /* rotation matrix for current state */ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 87a8385d3..4ee78516f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -734,10 +734,16 @@ MulticopterPositionControl::task_main() if (_local_pos_sp_pub > 0) { orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); - } else { + } else if (!_control_mode.flag_control_offboard_enabled) { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); } + /* Close fd to let offboard pos sp be advertised in mavlink receiver*/ + if (_control_mode.flag_control_offboard_enabled && _local_pos_sp_pub > 0) { + close(_local_pos_sp_pub); + _local_pos_sp_pub = -1; + } + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); -- cgit v1.2.3 From 33f98abf8046506e071845a78d6341b6c8749cf0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 20:16:17 +0200 Subject: Fill error report --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 89e6d3948..28eab49f2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2266,7 +2266,7 @@ int AttPosEKF::CheckAndBound() // Reset the filter if gyro offsets are excessive if (fabs(states[10]) > 1.0f || fabsf(states[11]) > 1.0f || fabsf(states[12]) > 1.0f) { - + FillErrorReport(&last_ekf_error); InitializeDynamic(velNED, magDeclination); // that's all we can do here, return -- cgit v1.2.3 From 2219dd3fc6586e7da8a1bc7b7514b97261ff8b23 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 20:16:41 +0200 Subject: Undo hacking --- src/modules/ekf_att_pos_estimator/estimator_21states.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp index 0f5a17a33..67bfec4ea 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp @@ -61,8 +61,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // Apply corrections for earths rotation rate and coning errors // * and + operators have been overloaded - correctedDelAng = dAngIMU;//correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); - correctedDelAng.z = 0; + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); // Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); if (rotationMag < 1e-12f) -- cgit v1.2.3 From 99dfef357b2f9228df54f0f26481c37204afa591 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 7 Jun 2014 12:54:04 -0700 Subject: fix extraction of path info from FTP request --- src/modules/mavlink/mavlink_ftp.cpp | 20 ++++++++++++++------ src/modules/mavlink/mavlink_ftp.h | 3 ++- 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 16f96f2cc..7a72835cc 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -33,6 +33,7 @@ #include #include +#include #include #include "mavlink_ftp.h" @@ -105,11 +106,14 @@ MavlinkFTP::_worker(Request *req) // check request CRC to make sure this is one of ours messageCRC = hdr->crc32; hdr->crc32 = 0; - if (crc32(req->data(), req->dataSize()) != messageCRC) { + if (crc32(req->rawData(), req->dataSize()) != messageCRC) { errorCode = kErrNoRequest; goto out; + printf("ftp: bad crc\n"); } + printf("ftp: opc %u size %u offset %u\n", hdr->opcode, hdr->size, hdr->offset); + switch (hdr->opcode) { case kCmdNone: break; @@ -157,7 +161,9 @@ out: // handle success vs. error if (errorCode == kErrNone) { hdr->opcode = kRspAck; + printf("FTP: ack\n"); } else { + printf("FTP: nak %u\n", errorCode); hdr->opcode = kRspNak; hdr->size = 1; hdr->data[0] = errorCode; @@ -177,10 +183,10 @@ MavlinkFTP::_reply(Request *req) // message is assumed to be already constructed in the request buffer, so generate the CRC hdr->crc32 = 0; - hdr->crc32 = crc32(req->data(), req->dataSize()); + hdr->crc32 = crc32(req->rawData(), req->dataSize()); // then pack and send the reply back to the request source - mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->data()); + mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->rawData()); } MavlinkFTP::ErrorCode @@ -190,6 +196,7 @@ MavlinkFTP::_workList(Request *req) DIR *dp = opendir(req->dataAsCString()); if (dp == nullptr) { + printf("FTP: can't open path '%s'\n", req->dataAsCString()); return kErrNotDir; } @@ -232,6 +239,7 @@ MavlinkFTP::_workList(Request *req) // copy the name, which we know will fit strcpy((char *)&hdr->data[offset], entry.d_name); + printf("FTP: list %s\n", entry.d_name); } closedir(dp); @@ -408,11 +416,11 @@ MavlinkFTP::Request::dataAsCString() { // guarantee nul termination if (header()->size < kMaxDataLength) { - data()[header()->size] = '\0'; + requestData()[header()->size] = '\0'; } else { - data()[kMaxDataLength - 1] = '\0'; + requestData()[kMaxDataLength - 1] = '\0'; } // and return data - return (char *)data(); + return (char *)&(header()->data[0]); } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index eab2a567a..6a2414613 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -159,8 +159,9 @@ private: } } + uint8_t *rawData() { return &_message.data[0]; } RequestHeader *header() { return reinterpret_cast(&_message.data[0]); } - uint8_t *data() { return &_message.data[0]; } + uint8_t *requestData() { return &(header()->data[0]); } unsigned dataSize() { return header()->size + sizeof(RequestHeader); } uint16_t sequence() const { return _message.seqnr; } -- cgit v1.2.3 From e8906619005a09d98fe68b04f2867c537f0895a8 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 7 Jun 2014 14:46:46 -0700 Subject: Don't queue empty work items. --- src/modules/mavlink/mavlink_ftp.cpp | 13 ++++++++----- src/modules/mavlink/mavlink_ftp.h | 25 +++++++++++++------------ 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 7a72835cc..d4d659d91 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -73,10 +73,13 @@ MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel) if (req != nullptr) { // decode the request - req->decode(channel, msg); + if (req->decode(msg, channel)) { - // and queue it for the worker - work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); + // and queue it for the worker + work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); + } else { + _qFree(req); + } } } @@ -112,7 +115,7 @@ MavlinkFTP::_worker(Request *req) printf("ftp: bad crc\n"); } - printf("ftp: opc %u size %u offset %u\n", hdr->opcode, hdr->size, hdr->offset); + printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); switch (hdr->opcode) { case kCmdNone: @@ -186,7 +189,7 @@ MavlinkFTP::_reply(Request *req) hdr->crc32 = crc32(req->rawData(), req->dataSize()); // then pack and send the reply back to the request source - mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->rawData()); + req->reply(); } MavlinkFTP::ErrorCode diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 6a2414613..f68dab98d 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -144,19 +144,18 @@ private: dq_entry_t entry; work_s work; }; - mavlink_channel_t channel; - - void decode(mavlink_channel_t fromChannel, mavlink_message_t *fromMessage) { - switch (fromMessage->msgid) { - - case MAVLINK_MSG_ID_ENCAPSULATED_DATA: - channel = fromChannel; - mavlink_msg_encapsulated_data_decode(fromMessage, &_message); - warnx("got enc data"); - break; - default: - warnx("unknown msg"); + + bool decode(mavlink_message_t *fromMessage, mavlink_channel_t fromChannel) { + if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) { + _channel = fromChannel; + mavlink_msg_encapsulated_data_decode(fromMessage, &_message); + return true; } + return false; + } + + void reply() { + mavlink_msg_encapsulated_data_send(_channel, sequence(), rawData()); } uint8_t *rawData() { return &_message.data[0]; } @@ -164,10 +163,12 @@ private: uint8_t *requestData() { return &(header()->data[0]); } unsigned dataSize() { return header()->size + sizeof(RequestHeader); } uint16_t sequence() const { return _message.seqnr; } + mavlink_channel_t &channel() { return _channel; } char *dataAsCString(); private: + mavlink_channel_t _channel; mavlink_encapsulated_data_t _message; }; -- cgit v1.2.3 From 9f754e0e9a2e2ad75a3fe5a15690cf542ce08e8a Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 14:05:35 +0200 Subject: U-blox driver rework, step 3 More sat info isolation Flush input after changing baudrate to allow driver restart w/o GNSS module restart --- src/drivers/gps/gps.cpp | 98 ++++++++++++++----------- src/drivers/gps/ubx.cpp | 119 ++++++++++++++----------------- src/drivers/gps/ubx.h | 10 +-- src/modules/uORB/topics/satellite_info.h | 17 +++-- 4 files changed, 126 insertions(+), 118 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index d2ffc30ba..d8e2bd797 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include @@ -106,8 +107,10 @@ private: bool _mode_changed; ///< flag that the GPS mode has changed gps_driver_mode_t _mode; ///< current mode GPS_Helper *_Helper; ///< instance of GPS parser - struct vehicle_gps_position_s _report; ///< uORB topic for gps position - orb_advert_t _report_pub; ///< uORB pub for gps position + struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position + orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position + struct satellite_info_s _report_sat_info; ///< uORB topic for satellite info + orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate bool _fake_gps; ///< fake gps output @@ -161,7 +164,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) : _mode_changed(false), _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), - _report_pub(-1), + _report_gps_pos_pub(-1), + _report_sat_info_pub(-1), _rate(0.0f), _fake_gps(fake_gps) { @@ -172,7 +176,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) : /* we need this potentially before it could be set in task_main */ g_dev = this; - memset(&_report, 0, sizeof(_report)); + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + memset(&_report_sat_info, 0, sizeof(_report_sat_info)); _debug_enabled = true; } @@ -271,33 +276,33 @@ GPS::task_main() if (_fake_gps) { - _report.timestamp_position = hrt_absolute_time(); - _report.lat = (int32_t)47.378301e7f; - _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)1200e3f; - _report.timestamp_variance = hrt_absolute_time(); - _report.s_variance_m_s = 10.0f; - _report.p_variance_m = 10.0f; - _report.c_variance_rad = 0.1f; - _report.fix_type = 3; - _report.eph_m = 0.9f; - _report.epv_m = 1.8f; - _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 0.0f; - _report.vel_e_m_s = 0.0f; - _report.vel_d_m_s = 0.0f; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = 0.0f; - _report.vel_ned_valid = true; + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = (int32_t)47.378301e7f; + _report_gps_pos.lon = (int32_t)8.538777e7f; + _report_gps_pos.alt = (int32_t)1200e3f; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.s_variance_m_s = 10.0f; + _report_gps_pos.p_variance_m = 10.0f; + _report_gps_pos.c_variance_rad = 0.1f; + _report_gps_pos.fix_type = 3; + _report_gps_pos.eph_m = 0.9f; + _report_gps_pos.epv_m = 1.8f; + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.vel_ned_valid = true; //no time and satellite information simulated if (!(_pub_blocked)) { - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (_report_gps_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } @@ -313,11 +318,11 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); + _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, UBX_ENABLE_NAV_SVINFO); break; case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); + _Helper = new MTK(_serial_fd, &_report_gps_pos); break; default: @@ -332,20 +337,33 @@ GPS::task_main() // GPS is obviously detected successfully, reset statistics _Helper->reset_update_rates(); - while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { + int helper_ret; + while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (helper_ret & 1) { + if (_report_gps_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + } + if (helper_ret & 2) { + if (_report_sat_info_pub > 0) { + orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, &_report_sat_info); + + } else { + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), &_report_sat_info); + } } } - last_rate_count++; + if (helper_ret & 1) { // consider only pos info updates for rate calculation */ + last_rate_count++; + } /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { @@ -357,7 +375,7 @@ GPS::task_main() } if (!_healthy) { - char *mode_str = "unknown"; + const char *mode_str = "unknown"; switch (_mode) { case GPS_DRIVER_MODE_UBX: @@ -447,11 +465,11 @@ GPS::print_info() warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - if (_report.timestamp_position != 0) { - warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type, - _report.satellites_used, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f); - warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m); + if (_report_gps_pos.timestamp_position != 0) { + warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); + warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph_m, (double)_report_gps_pos.epv_m); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); @@ -578,7 +596,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char *device_name = GPS_DEFAULT_UART_PORT; + const char *device_name = GPS_DEFAULT_UART_PORT; bool fake_gps = false; /* diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 462875b6c..dd47572b4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -65,9 +65,13 @@ #define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls #define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval -UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) + +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info) : _fd(fd), _gps_position(gps_position), + _satellite_info(satellite_info), + _enable_sat_info(enable_sat_info), _configured(false), _waiting_for_ack(false), _disable_cmd_last(0) @@ -84,14 +88,19 @@ UBX::configure(unsigned &baudrate) { _configured = false; /* try different baudrates */ - const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200}; - int baud_i; + unsigned baud_i; - for (baud_i = 0; baud_i < 5; baud_i++) { - baudrate = baudrates_to_try[baud_i]; + for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) { + baudrate = baudrates[baud_i]; set_baudrate(_fd, baudrate); + /* flush input and wait for at least 20 ms silence */ + decode_init(); + wait_for_ack(20); + decode_init(); + /* Send a CFG-PRT message to set the UBX protocol for in and out * and leave the baudrate as it is, we just want an ACK-ACK from this */ @@ -107,7 +116,7 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; cfg_prt_packet.baudRate = baudrate; cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; @@ -125,7 +134,7 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; @@ -144,7 +153,7 @@ UBX::configure(unsigned &baudrate) break; } - if (baud_i >= 5) { + if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) { return 1; } @@ -220,14 +229,14 @@ UBX::configure(unsigned &baudrate) return 1; } -#ifdef UBX_ENABLE_NAV_SVINFO - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); + if (_enable_sat_info) { + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SVINFO"); - return 1; + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: NAV SVINFO"); + return 1; + } } -#endif configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); @@ -244,14 +253,13 @@ int UBX::wait_for_ack(unsigned timeout) { _waiting_for_ack = true; - uint64_t time_started = hrt_absolute_time(); + uint64_t time_end = hrt_absolute_time() + timeout * 1000; - while (hrt_absolute_time() < time_started + timeout * 1000) { + while ((timeout = (time_end - hrt_absolute_time()) / 1000) > 0) { if (receive(timeout) > 0) { if (!_waiting_for_ack) { return 1; } - } else { return -1; // timeout or error receiving, or NAK } @@ -275,7 +283,7 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool handled = false; + int handled = 0; while (true) { @@ -290,7 +298,7 @@ UBX::receive(unsigned timeout) } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ if (handled) { - return 1; + return handled; } else { return -1; @@ -311,8 +319,7 @@ UBX::receive(unsigned timeout) /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { if (parse_char(buf[i]) > 0) { - if (handle_message() > 0) - handled = true; + handled |= handle_message(); } } } @@ -334,6 +341,7 @@ UBX::parse_char(uint8_t b) case UBX_DECODE_UNINIT: if (b == UBX_SYNC1) { _decode_state = UBX_DECODE_GOT_SYNC1; + //### printf("\nA"); } break; @@ -342,11 +350,13 @@ UBX::parse_char(uint8_t b) case UBX_DECODE_GOT_SYNC1: if (b == UBX_SYNC2) { _decode_state = UBX_DECODE_GOT_SYNC2; + //### printf("B"); } else { /* Second start symbol was wrong, reset state machine */ decode_init(); /* don't return error, it can be just false sync1 */ + //### printf("-"); } break; @@ -357,24 +367,28 @@ UBX::parse_char(uint8_t b) add_byte_to_checksum(b); _message_class = b; _decode_state = UBX_DECODE_GOT_CLASS; + //### printf("C"); break; case UBX_DECODE_GOT_CLASS: add_byte_to_checksum(b); _message_id = b; _decode_state = UBX_DECODE_GOT_MESSAGEID; + //### printf("D"); break; case UBX_DECODE_GOT_MESSAGEID: add_byte_to_checksum(b); _payload_size = b; //this is the first length byte _decode_state = UBX_DECODE_GOT_LENGTH1; + //### printf("E"); break; case UBX_DECODE_GOT_LENGTH1: add_byte_to_checksum(b); _payload_size += b << 8; // here comes the second byte of length _decode_state = UBX_DECODE_GOT_LENGTH2; + //### printf("F"); break; case UBX_DECODE_GOT_LENGTH2: @@ -389,6 +403,7 @@ UBX::parse_char(uint8_t b) if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes /* compare checksum */ if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { + //### printf("O\n"); return 1; // message received successfully } else { @@ -399,6 +414,7 @@ UBX::parse_char(uint8_t b) } else if (_rx_count < RECV_BUFFER_SIZE) { _rx_count++; + //### printf("."); } else { warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); @@ -489,65 +505,37 @@ UBX::handle_message() break; } -#ifdef UBX_ENABLE_NAV_SVINFO case UBX_MESSAGE_NAV_SVINFO: { //printf("GOT NAV_SVINFO\n"); + if (!_enable_sat_info) { // if satellite info processing not enabled: + break; // ignore and disable NAV-SVINFO message + } const int length_part1 = 8; gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; const int length_part2 = 12; gps_bin_nav_svinfo_part2_packet_t *packet_part2; - uint8_t satellites_used = 0; - int i; - //printf("Number of Channels: %d\n", packet_part1->numCh); - for (i = 0; i < packet_part1->numCh; i++) { - /* set pointer to sattelite_i information */ - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); + for (_satellite_info->count = 0; + _satellite_info->count < MIN(packet_part1->numCh, sizeof(_satellite_info->snr) / sizeof(_satellite_info->snr[0])); + _satellite_info->count++) { + /* set pointer to satellite_i information */ + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + _satellite_info->count* length_part2]); /* write satellite information to global storage */ - uint8_t sv_used = packet_part2->flags & 0x01; - - if (sv_used) { - /* count SVs used for NAV */ - satellites_used++; - } - - /* record info for all channels, whether or not the SV is used for NAV */ - _gps_position->satellite_used[i] = sv_used; - _gps_position->satellite_snr[i] = packet_part2->cno; - _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - _gps_position->satellite_prn[i] = packet_part2->svid; - //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); - } - - for (i = packet_part1->numCh; i < 20; i++) { - /* unused channels have to be set to zero for e.g. MAVLink */ - _gps_position->satellite_prn[i] = 0; - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; + _satellite_info->used[_satellite_info->count] = packet_part2->flags & 0x01; + _satellite_info->snr[_satellite_info->count] = packet_part2->cno; + _satellite_info->elevation[_satellite_info->count] = (uint8_t)(packet_part2->elev); + _satellite_info->azimuth[_satellite_info->count] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + _satellite_info->svid[_satellite_info->count] = packet_part2->svid; + //printf("SAT %d: %d %d %d %d\n", _satellite_info->count, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); } - /* Note: _gps_position->satellites_visible is taken from NAV-SOL now. */ - /* _gps_position->satellites_visible = satellites_used; */ // visible ~= used but we are interested in the used ones - /* TODO: satellites_used may be written into a new field after sat monitoring data got separated from _gps_position */ + _satellite_info->timestamp = hrt_absolute_time(); - if (packet_part1->numCh > 0) { - _gps_position->satellite_info_available = true; - - } else { - _gps_position->satellite_info_available = false; - } - - _gps_position->timestamp_satellites = hrt_absolute_time(); - - ret = 1; + ret = 2; break; } -#endif /* #ifdef UBX_ENABLE_NAV_SVINFO */ case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -650,7 +638,6 @@ UBX::handle_message() warnx("ubx: not acknowledged"); /* configuration obviously not successful */ _waiting_for_ack = false; - ret = -1; break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index b844c315e..90aeffedc 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -48,9 +48,7 @@ #include "gps_helper.h" -/* TODO: processing of NAV_SVINFO disabled, has to be re-written as an optional feature */ -/* TODO: make this a command line option, allocate buffer dynamically */ -#undef UBX_ENABLE_NAV_SVINFO +#define UBX_ENABLE_NAV_SVINFO 1 /* TODO: make this a command line option, allocate buffer(s) dynamically */ #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 @@ -378,7 +376,7 @@ typedef enum { /* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */ /* TODO: make this a command line option, allocate buffer dynamically */ #define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */ -#ifdef UBX_ENABLE_NAV_SVINFO +#if (UBX_ENABLE_NAV_SVINFO != 0) #define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) #else #define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) @@ -387,7 +385,7 @@ typedef enum { class UBX : public GPS_Helper { public: - UBX(const int &fd, struct vehicle_gps_position_s *gps_position); + UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info); ~UBX(); int receive(unsigned timeout); int configure(unsigned &baudrate); @@ -434,6 +432,8 @@ private: int _fd; struct vehicle_gps_position_s *_gps_position; + struct satellite_info_s *_satellite_info; + bool _enable_sat_info; bool _configured; bool _waiting_for_ack; uint8_t _message_class_needed; diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h index 5f799eda4..37c2faa96 100644 --- a/src/modules/uORB/topics/satellite_info.h +++ b/src/modules/uORB/topics/satellite_info.h @@ -53,14 +53,17 @@ /** * GNSS Satellite Info. */ + +#define SAT_INFO_MAX_SATELLITES 20 + struct satellite_info_s { - uint64_t timestamp; /**< Timestamp of satellite information */ - uint8_t count; /**< Number of satellites in satellite_...[] arrays */ - uint8_t svid[20]; /**< Space vehicle ID [1..255], see scheme below */ - uint8_t used[20]; /**< 0: Satellite not used, 1: used for navigation */ - uint8_t elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ + uint64_t timestamp; /**< Timestamp of satellite info */ + uint8_t count; /**< Number of satellites in satellite info */ + uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ + uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ + uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ }; /** -- cgit v1.2.3 From 018eeeecad748959a8afa4845a536e897a57249d Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 15:56:42 +0200 Subject: Merged ubx driver part of pull request #1031 --- src/drivers/gps/ubx.cpp | 15 +++++++++++---- src/drivers/gps/ubx.h | 2 ++ 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index dd47572b4..0708e23b4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -74,6 +74,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _enable_sat_info(enable_sat_info), _configured(false), _waiting_for_ack(false), + _got_posllh(false), + _got_velned(false), _disable_cmd_last(0) { decode_init(); @@ -208,7 +210,7 @@ UBX::configure(unsigned &baudrate) return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("MSG CFG FAIL: NAV TIMEUTC"); @@ -286,9 +288,10 @@ UBX::receive(unsigned timeout) int handled = 0; while (true) { + bool ready_to_return = _configured ? (_got_posllh && _got_velned) : handled; /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout); + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ @@ -297,7 +300,9 @@ UBX::receive(unsigned timeout) } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ - if (handled) { + if (ready_to_return) { + _got_posllh = false; + _got_velned = false; return handled; } else { @@ -456,7 +461,8 @@ UBX::handle_message() _rate_count_lat_lon++; - ret = 1; + _got_posllh = true; + ret = 1; break; } @@ -552,6 +558,7 @@ UBX::handle_message() _rate_count_vel++; + _got_velned = true; ret = 1; break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 90aeffedc..50df3ada2 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -436,6 +436,8 @@ private: bool _enable_sat_info; bool _configured; bool _waiting_for_ack; + bool _got_posllh; + bool _got_velned; uint8_t _message_class_needed; uint8_t _message_id_needed; ubx_decode_state_t _decode_state; -- cgit v1.2.3 From 3538773b99d4231bf8a64e9c257cfe0299a93021 Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 16:05:04 +0200 Subject: Initialize rate values Initialize rate values (else invalid rates wil be displayed during the first measurement interval). --- src/drivers/gps/gps_helper.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index d14a95afe..3623397b2 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -62,8 +62,8 @@ protected: uint8_t _rate_count_lat_lon; uint8_t _rate_count_vel; - float _rate_lat_lon; - float _rate_vel; + float _rate_lat_lon = 0.0f; + float _rate_vel = 0.0f; uint64_t _interval_rate_start; }; -- cgit v1.2.3 From 07458b284d4cea48bc3b6e0cb5dcaba1c2c1d4d6 Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 17:54:50 +0200 Subject: Gps driver: make enable_sat_info a command line option --- src/drivers/gps/gps.cpp | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index d8e2bd797..f4d2f4b44 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -83,7 +83,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char *uart_path, bool fake_gps); + GPS(const char *uart_path, bool fake_gps, bool enable_sat_info); virtual ~GPS(); virtual int init(); @@ -113,6 +113,7 @@ private: orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate bool _fake_gps; ///< fake gps output + bool _enable_sat_info; ///< enable sat info /** @@ -157,7 +158,7 @@ GPS *g_dev; } -GPS::GPS(const char *uart_path, bool fake_gps) : +GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -167,7 +168,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) : _report_gps_pos_pub(-1), _report_sat_info_pub(-1), _rate(0.0f), - _fake_gps(fake_gps) + _fake_gps(fake_gps), + _enable_sat_info(enable_sat_info) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -318,7 +320,7 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, UBX_ENABLE_NAV_SVINFO); + _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, _enable_sat_info); break; case GPS_DRIVER_MODE_MTK: @@ -464,6 +466,7 @@ GPS::print_info() } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + warnx("sat info: %s", (_enable_sat_info) ? "enabled" : "disabled"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, @@ -487,7 +490,7 @@ namespace gps GPS *g_dev; -void start(const char *uart_path, bool fake_gps); +void start(const char *uart_path, bool fake_gps, bool enable_sat_info); void stop(); void test(); void reset(); @@ -497,7 +500,7 @@ void info(); * Start the driver. */ void -start(const char *uart_path, bool fake_gps) +start(const char *uart_path, bool fake_gps, bool enable_sat_info) { int fd; @@ -505,7 +508,7 @@ start(const char *uart_path, bool fake_gps) errx(1, "already started"); /* create the driver */ - g_dev = new GPS(uart_path, fake_gps); + g_dev = new GPS(uart_path, fake_gps, enable_sat_info); if (g_dev == nullptr) goto fail; @@ -598,6 +601,7 @@ gps_main(int argc, char *argv[]) /* set to default */ const char *device_name = GPS_DEFAULT_UART_PORT; bool fake_gps = false; + bool enable_sat_info = false; /* * Start/load the driver. @@ -619,7 +623,13 @@ gps_main(int argc, char *argv[]) fake_gps = true; } - gps::start(device_name, fake_gps); + /* Detect sat info option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-s")) + enable_sat_info = true; + } + + gps::start(device_name, fake_gps, enable_sat_info); } if (!strcmp(argv[1], "stop")) @@ -644,5 +654,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]"); } -- cgit v1.2.3 From 2235a86a66c1c9dd70c43323f8b801d7e03e7436 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Jun 2014 18:47:38 +0200 Subject: Support link forwarding --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 76593881d..02002b55f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,7 +3,7 @@ # USB MAVLink start # -mavlink start -r 10000 -d /dev/ttyACM0 +mavlink start -r 10000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 usleep 100000 -- cgit v1.2.3 From 4b70a0d04687ee21c21113acea73fc96088a0d86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Jun 2014 18:48:31 +0200 Subject: Added include order warning --- mavlink/include/mavlink/v1.0/common/common.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h index 020211d40..de6e22011 100644 --- a/mavlink/include/mavlink/v1.0/common/common.h +++ b/mavlink/include/mavlink/v1.0/common/common.h @@ -5,6 +5,10 @@ #ifndef COMMON_H #define COMMON_H +#ifndef MAVLINK_H + #error Wrong include order: common.h MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set all defines from mavlink.h manually. +#endif + #ifdef __cplusplus extern "C" { #endif -- cgit v1.2.3 From dfe71b615c3d88d223a91559912b0c7d7f0cad0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Jun 2014 18:50:36 +0200 Subject: MAVLink helper function fixes --- mavlink/include/mavlink/v1.0/checksum.h | 2 +- mavlink/include/mavlink/v1.0/mavlink_helpers.h | 21 ++++++++++++++------- mavlink/include/mavlink/v1.0/mavlink_types.h | 4 ++-- 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/mavlink/include/mavlink/v1.0/checksum.h b/mavlink/include/mavlink/v1.0/checksum.h index 948e080a1..7d9b6ac0c 100644 --- a/mavlink/include/mavlink/v1.0/checksum.h +++ b/mavlink/include/mavlink/v1.0/checksum.h @@ -73,7 +73,7 @@ static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) * @param data new bytes to hash * @param crcAccum the already accumulated checksum **/ -static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length) +static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length) { const uint8_t *p = (const uint8_t *)pBuffer; while (length--) { diff --git a/mavlink/include/mavlink/v1.0/mavlink_helpers.h b/mavlink/include/mavlink/v1.0/mavlink_helpers.h index 96672f847..1639a830b 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_helpers.h +++ b/mavlink/include/mavlink/v1.0/mavlink_helpers.h @@ -73,7 +73,6 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui #endif { // This code part is the same for all messages; - uint16_t checksum; msg->magic = MAVLINK_STX; msg->len = length; msg->sysid = system_id; @@ -81,12 +80,13 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui // One sequence number per component msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN); + msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN); + crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len); #if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); + crc_accumulate(crc_extra, &msg->checksum); #endif - mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); - mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); + mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF); + mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8); return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -133,7 +133,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint buf[4] = mavlink_system.compid; buf[5] = msgid; status->current_tx_seq++; - checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); crc_accumulate_buffer(&checksum, packet, length); #if MAVLINK_CRC_EXTRA crc_accumulate(crc_extra, &checksum); @@ -158,6 +158,7 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m ck[0] = (uint8_t)(msg->checksum & 0xFF); ck[1] = (uint8_t)(msg->checksum >> 8); + // XXX use the right sequence here MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); @@ -172,7 +173,13 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m */ MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) { - memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len); + memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); + + uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); + + ck[0] = (uint8_t)(msg->checksum & 0xFF); + ck[1] = (uint8_t)(msg->checksum >> 8); + return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; } diff --git a/mavlink/include/mavlink/v1.0/mavlink_types.h b/mavlink/include/mavlink/v1.0/mavlink_types.h index 4019c619e..43658e629 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_types.h +++ b/mavlink/include/mavlink/v1.0/mavlink_types.h @@ -28,6 +28,7 @@ #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) +#pragma pack(push, 1) typedef struct param_union { union { float param_float; @@ -62,13 +63,12 @@ typedef struct __mavlink_message { uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; } mavlink_message_t; - typedef struct __mavlink_extended_message { mavlink_message_t base_msg; int32_t extended_payload_len; ///< Length of extended payload if any uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; } mavlink_extended_message_t; - +#pragma pack(pop) typedef enum { MAVLINK_TYPE_CHAR = 0, -- cgit v1.2.3 From f84e18f27a84254c9760a771f8ad91f864ba25fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Jun 2014 18:51:17 +0200 Subject: Formatting and some ftp drive-by --- src/modules/mavlink/mavlink_main.h | 172 ++++++++++++++++++++----------------- 1 file changed, 94 insertions(+), 78 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 40edc4b85..f0ca028a8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -123,27 +123,41 @@ public: /** * Display the mavlink status. */ - void status(); + void status(); - static int stream(int argc, char *argv[]); + static int stream(int argc, char *argv[]); - static int instance_count(); + static int instance_count(); - static Mavlink *new_instance(); + static Mavlink *new_instance(); - static Mavlink *get_instance(unsigned instance); + static Mavlink *get_instance(unsigned instance); - static Mavlink *get_instance_for_device(const char *device_name); + static Mavlink *get_instance_for_device(const char *device_name); - static int destroy_all_instances(); + static int destroy_all_instances(); - static bool instance_exists(const char *device_name, Mavlink *self); + static bool instance_exists(const char *device_name, Mavlink *self); - static void forward_message(mavlink_message_t *msg, Mavlink *self); + static void forward_message(mavlink_message_t *msg, Mavlink *self); - static int get_uart_fd(unsigned index); + static int get_uart_fd(unsigned index); - int get_uart_fd(); + int get_uart_fd(); + + /** + * Get the MAVLink system id. + * + * @return The system ID of this vehicle + */ + int get_system_id(); + + /** + * Get the MAVLink component id. + * + * @return The component ID of this vehicle + */ + int get_component_id(); const char *_device_name; @@ -153,30 +167,30 @@ public: MAVLINK_MODE_CAMERA }; - void set_mode(enum MAVLINK_MODE); - enum MAVLINK_MODE get_mode() { return _mode; } + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _hil_enabled; } + bool get_hil_enabled() { return _hil_enabled; } - bool get_use_hil_gps() { return _use_hil_gps; } + bool get_use_hil_gps() { return _use_hil_gps; } - bool get_flow_control_enabled() { return _flow_control_enabled; } + bool get_flow_control_enabled() { return _flow_control_enabled; } - bool get_forwarding_on() { return _forwarding_on; } + bool get_forwarding_on() { return _forwarding_on; } /** * Handle waypoint related messages. */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); + void mavlink_wpm_message_handler(const mavlink_message_t *msg); - static int start_helper(int argc, char *argv[]); + static int start_helper(int argc, char *argv[]); /** * Handle parameter related messages. */ - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); /** * Enable / disable Hardware in the Loop simulation mode. @@ -186,90 +200,93 @@ public: * requested change could not be made or was * redundant. */ - int set_hil_enabled(bool hil_enabled); + int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); - int get_instance_id(); + int get_instance_id(); /** * Enable / disable hardware flow control. * * @param enabled True if hardware flow control should be enabled */ - int enable_flow_control(bool enabled); + int enable_flow_control(bool enabled); - mavlink_channel_t get_channel(); + const mavlink_channel_t get_channel(); - bool _task_should_exit; /**< if true, mavlink task should exit */ + bool _task_should_exit; /**< if true, mavlink task should exit */ - int get_mavlink_fd() { return _mavlink_fd; } + int get_mavlink_fd() { return _mavlink_fd; } /* Functions for waiting to start transmission until message received. */ - void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } - bool get_has_received_messages() { return _received_messages; } - void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } - bool get_wait_to_transmit() { return _wait_to_transmit; } - bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + + bool message_buffer_write(void *ptr, int size); protected: - Mavlink *next; + Mavlink *next; private: - int _instance_id; + int _instance_id; - int _mavlink_fd; - bool _task_running; + int _mavlink_fd; + bool _task_running; /* states */ - bool _hil_enabled; /**< Hardware In the Loop mode */ - bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ - bool _is_usb_uart; /**< Port is USB */ - bool _wait_to_transmit; /**< Wait to transmit until received messages. */ - bool _received_messages; /**< Whether we've received valid mavlink messages. */ + bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ - unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ - MavlinkOrbSubscription *_subscriptions; - MavlinkStream *_streams; + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; - orb_advert_t _mission_pub; - struct mission_s mission; - MAVLINK_MODE _mode; + orb_advert_t _mission_pub; + struct mission_s mission; + MAVLINK_MODE _mode; - uint8_t _mavlink_wpm_comp_id; - mavlink_channel_t _channel; + uint8_t _mavlink_wpm_comp_id; + mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; - unsigned int _total_counter; + unsigned int _total_counter; - pthread_t _receive_thread; + pthread_t _receive_thread; /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; + mavlink_wpm_storage _wpm_s; + mavlink_wpm_storage *_wpm; - bool _verbose; - bool _forwarding_on; - bool _passing_on; - int _uart_fd; - int _baudrate; - int _datarate; + bool _verbose; + bool _forwarding_on; + bool _passing_on; + bool _ftp_on; + int _uart_fd; + int _baudrate; + int _datarate; /** * If the queue index is not at 0, the queue sending * logic will send parameters from the current index * to len - 1, the end of the param list. */ - unsigned int _mavlink_param_queue_index; + unsigned int _mavlink_param_queue_index; - bool mavlink_link_termination_allowed; + bool mavlink_link_termination_allowed; - char *_subscribe_to_stream; - float _subscribe_to_stream_rate; + char *_subscribe_to_stream; + float _subscribe_to_stream_rate; - bool _flow_control_enabled; + bool _flow_control_enabled; struct mavlink_message_buffer { int write_ptr; @@ -277,11 +294,12 @@ private: int size; char *data; }; - mavlink_message_buffer _message_buffer; - pthread_mutex_t _message_buffer_mutex; + mavlink_message_buffer _message_buffer; - perf_counter_t _loop_perf; /**< loop performance counter */ + pthread_mutex_t _message_buffer_mutex; + + perf_counter_t _loop_perf; /**< loop performance counter */ /** * Send one parameter. @@ -289,7 +307,7 @@ private: * @param param The parameter id to send. * @return zero on success, nonzero on failure. */ - int mavlink_pm_send_param(param_t param); + int mavlink_pm_send_param(param_t param); /** * Send one parameter identified by index. @@ -297,7 +315,7 @@ private: * @param index The index of the parameter to send. * @return zero on success, nonzero else. */ - int mavlink_pm_send_param_for_index(uint16_t index); + int mavlink_pm_send_param_for_index(uint16_t index); /** * Send one parameter identified by name. @@ -305,14 +323,14 @@ private: * @param name The index of the parameter to send. * @return zero on success, nonzero else. */ - int mavlink_pm_send_param_for_name(const char *name); + int mavlink_pm_send_param_for_name(const char *name); /** * Send a queue of parameters, one parameter per function call. * * @return zero on success, nonzero on failure */ - int mavlink_pm_queued_send(void); + int mavlink_pm_queued_send(void); /** * Start sending the parameter queue. @@ -322,12 +340,12 @@ private: * mavlink_pm_queued_send(). * @see mavlink_pm_queued_send() */ - void mavlink_pm_start_queued_send(); + void mavlink_pm_start_queued_send(); - void mavlink_update_system(); + void mavlink_update_system(); - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); + void mavlink_waypoint_eventloop(uint64_t now); + void mavlink_wpm_send_waypoint_reached(uint16_t seq); void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); @@ -354,8 +372,6 @@ private: int message_buffer_is_empty(); - bool message_buffer_write(void *ptr, int size); - int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); -- cgit v1.2.3 From a103fef948b7f239afef21a8d0f848151891b409 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Jun 2014 18:51:35 +0200 Subject: Fixed threading and transmission issues for FTP --- src/modules/mavlink/mavlink_ftp.cpp | 4 +-- src/modules/mavlink/mavlink_ftp.h | 33 +++++++++++++---- src/modules/mavlink/mavlink_main.cpp | 62 ++++++++++++++++++++++++++------ src/modules/mavlink/mavlink_receiver.cpp | 2 +- 4 files changed, 80 insertions(+), 21 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index d4d659d91..8c29043e0 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -64,7 +64,7 @@ MavlinkFTP::MavlinkFTP() } void -MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel) +MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg) { // get a free request auto req = _dqFree(); @@ -73,7 +73,7 @@ MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel) if (req != nullptr) { // decode the request - if (req->decode(msg, channel)) { + if (req->decode(mavlink, msg)) { // and queue it for the worker work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index f68dab98d..0869a5fdb 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -64,8 +64,8 @@ public: static MavlinkFTP *getServer(); // static interface - void handle_message(mavlink_message_t *msg, - mavlink_channel_t channel); + void handle_message(Mavlink* mavlink, + mavlink_message_t *msg); private: @@ -145,9 +145,9 @@ private: work_s work; }; - bool decode(mavlink_message_t *fromMessage, mavlink_channel_t fromChannel) { + bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) { - _channel = fromChannel; + _mavlink = mavlink; mavlink_msg_encapsulated_data_decode(fromMessage, &_message); return true; } @@ -155,7 +155,26 @@ private: } void reply() { - mavlink_msg_encapsulated_data_send(_channel, sequence(), rawData()); + + // XXX the proper way would be an IOCTL / uORB call, rather than exploiting the + // flat memory architecture, as we're operating between threads here. + mavlink_message_t msg; + msg.checksum = 0; + unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), + _mavlink->get_channel(), &msg, sequence(), rawData()); + // unsigned len = mavlink_msg_system_time_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), + // _mavlink->get_channel(), &msg, 255, 255); + + if (!_mavlink->message_buffer_write(&msg, len+2)) { + warnx("FTP TX ERR"); + } else { + warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", + _mavlink->get_system_id(), + _mavlink->get_component_id(), + _mavlink->get_channel(), + len, + msg.checksum); + } } uint8_t *rawData() { return &_message.data[0]; } @@ -163,12 +182,12 @@ private: uint8_t *requestData() { return &(header()->data[0]); } unsigned dataSize() { return header()->size + sizeof(RequestHeader); } uint16_t sequence() const { return _message.seqnr; } - mavlink_channel_t &channel() { return _channel; } + mavlink_channel_t channel() { return _mavlink->get_channel(); } char *dataAsCString(); private: - mavlink_channel_t _channel; + Mavlink *_mavlink; mavlink_encapsulated_data_t _message; }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e300be074..066d25bf6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -83,6 +83,10 @@ #include "mavlink_rate_limiter.h" #include "mavlink_commands.h" +#ifndef MAVLINK_CRC_EXTRA + #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#endif + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -114,6 +118,7 @@ static uint64_t last_write_try_times[6] = {0}; void mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { + Mavlink *instance; switch (channel) { @@ -198,7 +203,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length ssize_t ret = write(uart, ch, desired); if (ret != desired) { - warnx("TX FAIL"); + // XXX overflow perf } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } @@ -230,6 +235,7 @@ Mavlink::Mavlink() : _verbose(false), _forwarding_on(false), _passing_on(false), + _ftp_on(false), _uart_fd(-1), _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), @@ -453,7 +459,7 @@ Mavlink::get_instance_id() return _instance_id; } -mavlink_channel_t +const mavlink_channel_t Mavlink::get_channel() { return _channel; @@ -536,6 +542,16 @@ void Mavlink::mavlink_update_system(void) _use_hil_gps = (bool)use_hil_gps; } +int Mavlink::get_system_id() +{ + return mavlink_system.sysid; +} + +int Mavlink::get_component_id() +{ + return mavlink_system.compid; +} + int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ @@ -1649,11 +1665,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) int Mavlink::message_buffer_init(int size) { + _message_buffer.size = size; _message_buffer.write_ptr = 0; _message_buffer.read_ptr = 0; _message_buffer.data = (char*)malloc(_message_buffer.size); - return (_message_buffer.data == 0) ? ERROR : OK; + + int ret; + if (_message_buffer.data == 0) { + ret = ERROR; + _message_buffer.size = 0; + } else { + ret = OK; + } + + return ret; } void @@ -1781,7 +1807,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1837,6 +1863,10 @@ Mavlink::task_main(int argc, char *argv[]) _wait_to_transmit = true; break; + case 'x': + _ftp_on = true; + break; + default: err_flag = true; break; @@ -1902,9 +1932,9 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_init(&_logbuffer, 5); /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ - if (_passing_on) { + if (_passing_on || _ftp_on) { /* initialize message buffer if multiplexing is on */ - if (OK != message_buffer_init(500)) { + if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN)) { errx(1, "can't allocate message buffer, exiting"); } @@ -2064,8 +2094,8 @@ Mavlink::task_main(int argc, char *argv[]) } } - /* pass messages from other UARTs */ - if (_passing_on) { + /* pass messages from other UARTs or FTP worker */ + if (_passing_on || _ftp_on) { bool is_part; void *read_ptr; @@ -2076,11 +2106,21 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { + + // int oldseq = mavlink_get_channel_status(get_channel())->current_tx_seq; + + const mavlink_message_t* msg = (const mavlink_message_t*)read_ptr; /* write first part of buffer */ - _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + _mavlink_resend_uart(_channel, msg); + + // mavlink_get_channel_status(get_channel())->current_tx_seq = oldseq; + // mavlink_msg_system_time_send(get_channel(), 255, 255); + message_buffer_mark_read(available); + /* write second part of buffer if there is some */ + // XXX this doesn't quite work, as the resend UART call assumes a continous block if (is_part) { /* guard get ptr by mutex */ pthread_mutex_lock(&_message_buffer_mutex); @@ -2139,7 +2179,7 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ close(_mavlink_fd); - if (_passing_on) { + if (_passing_on || _ftp_on) { message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } @@ -2281,7 +2321,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4f2c4ca85..4a244815a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -154,7 +154,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; case MAVLINK_MSG_ID_ENCAPSULATED_DATA: - MavlinkFTP::getServer()->handle_message(msg, _mavlink->get_channel()); + MavlinkFTP::getServer()->handle_message(_mavlink, msg); break; default: -- cgit v1.2.3 From 0ef66054988d8ef8053b4f64fee6454fefc04aef Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 8 Jun 2014 10:13:55 -0700 Subject: Fix packing of directory entries in the List reply --- src/modules/mavlink/mavlink_ftp.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 8c29043e0..bfee17342 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -242,6 +242,7 @@ MavlinkFTP::_workList(Request *req) // copy the name, which we know will fit strcpy((char *)&hdr->data[offset], entry.d_name); + offset += strlen(entry.d_name) + 1; printf("FTP: list %s\n", entry.d_name); } -- cgit v1.2.3 From a6cf04a6ff623b7d39a97c70f837198b6c064f5b Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 8 Jun 2014 14:08:22 -0400 Subject: Create system state entry in dataman - ATTN Anton Create persistent system state id for data manager to store system state that will persist across resets. --- ROMFS/px4fmu_common/init.d/rcS | 10 +++--- src/modules/dataman/dataman.c | 50 ++++++++++++++++++++++++++--- src/modules/dataman/dataman.h | 26 ++++++++++++++- src/modules/mavlink/mavlink_main.cpp | 54 ++++++++++++++++++++++++++++++-- src/modules/navigator/navigator_main.cpp | 9 ++++++ 5 files changed, 136 insertions(+), 13 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6d06f897a..13c08f405 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -402,6 +402,11 @@ then fi fi + # + # Start the datamanager + # + dataman start + # # MAVLink # @@ -544,11 +549,6 @@ then fi fi - # - # Start the datamanager - # - dataman start - # # Start the navigator # diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 1a65313e8..96dc98f2a 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -61,6 +61,8 @@ __EXPORT int dataman_main(int argc, char *argv[]); __EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen); __EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen); __EXPORT int dm_clear(dm_item_t item); +__EXPORT void dm_lock(dm_item_t item); +__EXPORT void dm_unlock(dm_item_t item); __EXPORT int dm_restart(dm_reset_reason restart_type); /** Types of function calls supported by the worker task */ @@ -113,12 +115,17 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_FENCE_POINTS_MAX, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, - DM_KEY_WAYPOINTS_ONBOARD_MAX + DM_KEY_WAYPOINTS_ONBOARD_MAX, + DM_KEY_SYSTEM_STATE_MAX }; /* Table of offset for index 0 of each item type */ static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; +/* Item type lock mutexes */ +static sem_t *g_item_locks[DM_KEY_NUM_KEYS]; +static sem_t g_sys_state_mutex; + /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; static const char *k_data_manager_device_path = "/fs/microsd/dataman"; @@ -138,22 +145,22 @@ static work_q_t g_work_q; /* pending work items. To be consumed by worker thread sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ sem_t g_init_sema; -static bool g_task_should_exit; /**< if true, dataman task should exit */ +static bool g_task_should_exit; /**< if true, dataman task should exit */ #define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */ static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */ static void init_q(work_q_t *q) { - sq_init(&(q->q)); /* Initialize the NuttX queue structure */ + sq_init(&(q->q)); /* Initialize the NuttX queue structure */ sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ - q->size = q->max_size = 0; /* Queue is initially empty */ + q->size = q->max_size = 0; /* Queue is initially empty */ } static inline void destroy_q(work_q_t *q) { - sem_destroy(&(q->mutex)); /* Destroy the queue lock */ + sem_destroy(&(q->mutex)); /* Destroy the queue lock */ } static inline void @@ -567,6 +574,32 @@ dm_clear(dm_item_t item) return enqueue_work_item_and_wait_for_result(work); } +__EXPORT void +dm_lock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if ((g_fd < 0) || g_task_should_exit) + return; + if (item >= DM_KEY_NUM_KEYS) + return; + if (g_item_locks[item]) { + sem_wait(g_item_locks[item]); + } +} + +__EXPORT void +dm_unlock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if ((g_fd < 0) || g_task_should_exit) + return; + if (item >= DM_KEY_NUM_KEYS) + return; + if (g_item_locks[item]) { + sem_post(g_item_locks[item]); + } +} + /* Tell the data manager about the type of the last reset */ __EXPORT int dm_restart(dm_reset_reason reason) @@ -607,6 +640,12 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < dm_number_of_funcs; i++) g_func_counts[i] = 0; + /* Initialize the item type locks, for now only DM_KEY_SYSTEM_STATE supports locking */ + sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */ + for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) + g_item_locks[i] = NULL; + g_item_locks[DM_KEY_SYSTEM_STATE] = &g_sys_state_mutex; + g_task_should_exit = false; init_q(&g_work_q); @@ -742,6 +781,7 @@ task_main(int argc, char *argv[]) destroy_q(&g_work_q); destroy_q(&g_free_q); sem_destroy(&g_work_queued_sema); + sem_destroy(&g_sys_state_mutex); return 0; } diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 1dfb26f73..f2b6cd4d3 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -53,6 +53,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ + DM_KEY_SYSTEM_STATE, /* Persistent system state storage */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -62,7 +63,8 @@ extern "C" { DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED + DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_SYSTEM_STATE_MAX = 1 }; /** Data persistence levels */ @@ -101,6 +103,18 @@ extern "C" { size_t buflen /* Length in bytes of data to retrieve */ ); + /** Lock all items of this type */ + __EXPORT void + dm_lock( + dm_item_t item /* The item type to clear */ + ); + + /** Unlock all items of this type */ + __EXPORT void + dm_unlock( + dm_item_t item /* The item type to clear */ + ); + /** Erase all items of this type */ __EXPORT int dm_clear( @@ -113,6 +127,16 @@ extern "C" { dm_reset_reason restart_type /* The last reset type */ ); + /* NOTE: The following structure defines the persistent system state data stored in the single + entry DM_KEY_SYSTEM_STATE_KEY item type. It contains global system state information that + needs to survive restarts. This definition is application specific so it doesn't really belong + in this header, but till I find it a better home here it is */ + + typedef struct { + char current_offboard_waypoint_id; /* the index of the active offboard waypoint data */ + /* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */ + } persistent_system_state_t; + #ifdef __cplusplus } #endif diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e300be074..cb1b762a7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -952,6 +952,8 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { + persistent_system_state_t sys_state; + state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; state->current_state = MAVLINK_WPM_STATE_IDLE; @@ -962,6 +964,10 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->timestamp_last_send_request = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->current_dataman_id = 0; + if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { + if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1)) + state->current_dataman_id = sys_state.current_offboard_waypoint_id; + } } /* @@ -1428,12 +1434,38 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mission.dataman_id = 0; } - if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + if (dm_write(dm_next, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, len) != len) { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } + // offboard mission data saved correctly, now update the persistent system state + { + persistent_system_state_t state; + bool dm_result; + // Since we are doing a read-modify-write we must lock the item type + dm_lock(DM_KEY_SYSTEM_STATE); + // first read in the current state data. There may eventually be data other than the offboard index + // and we must preserve it + if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) { + // Not sure how to handle this? It means that either the item was never + // written, or fields have been added to the system state struct. In any case + // fields that may not be ours need to be initialized to sane values. + // For now the offboard index is the only field, so for now there + // is nothing to do here. + } + state.current_offboard_waypoint_id = mission.dataman_id; + dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); + dm_unlock(DM_KEY_SYSTEM_STATE); + if (dm_result) { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; + } + } + // if (wp.current) { // warnx("current is: %d", wp.seq); @@ -1485,7 +1517,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) publish_mission(); if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + persistent_system_state_t state; + bool dm_result; + dm_lock(DM_KEY_SYSTEM_STATE); + // first read in the current state data. There may eventually be data other than the offboard index + // and we must preserve it + if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) { + // Not sure how to handle this? It means that either the item was never + // written, or fields have been added to the system state struct. In any case + // fields that may not be ours need to be initialized to sane values. + // For now the offboard index is the only field, so we can deal with it here. + } + state.current_offboard_waypoint_id = -1; + dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) == sizeof(state); + dm_unlock(DM_KEY_SYSTEM_STATE); + if (dm_result) { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + } else { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + } } else { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 06e0c5904..04ea7da0e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -603,6 +603,15 @@ Navigator::task_main() warnx("Could not clear geofence"); } +#if 0 // *** UNTESTED... Anton, this is for you + /* Get the last offboard mission id */ + persistent_system_state_t sys_state; + if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { + if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1)) + _mission.set_offboard_dataman_id(sys_state.current_offboard_waypoint_id); + } +#endif + /* * do subscriptions */ -- cgit v1.2.3 From 0a86fd0d9f3f3ba1a007ff54e1d498e7735d4aa5 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Sun, 8 Jun 2014 15:21:02 -0400 Subject: Changed struct name used for local_pos_sp --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 4ee78516f..6a105762b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -118,7 +118,7 @@ private: int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ - int _offboard_control_pos_sp_sub; /**< offboard control position setpoint */ + int _local_pos_sp_sub; /**< offboard local position setpoint */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ @@ -133,7 +133,6 @@ private: struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ - struct vehicle_local_position_setpoint_s _offboard_control_pos_sp; /**< offboard control position setpoint */ struct { @@ -280,7 +279,6 @@ MulticopterPositionControl::MulticopterPositionControl() : memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); - memset(&_offboard_control_pos_sp, 0, sizeof(_offboard_control_pos_sp)); memset(&_ref_pos, 0, sizeof(_ref_pos)); @@ -532,7 +530,7 @@ MulticopterPositionControl::task_main() _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _offboard_control_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); parameters_update(true); @@ -671,26 +669,25 @@ MulticopterPositionControl::task_main() } else if (_control_mode.flag_control_offboard_enabled) { /* Offboard control */ - /* Team elikos doesn't really know what it is doing */ bool updated; - orb_check(_offboard_control_pos_sp_sub, &updated); + orb_check(_local_pos_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), _offboard_control_pos_sp_sub, - &_offboard_control_pos_sp); + orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, + &_local_pos_sp); } /* Make sure position control is selected i.e. not only velocity control */ if (_control_mode.flag_control_position_enabled) { - _pos_sp(0) = _offboard_control_pos_sp.x; - _pos_sp(1) = _offboard_control_pos_sp.y; + _pos_sp(0) = _local_pos_sp.x; + _pos_sp(1) = _local_pos_sp.y; } if (_control_mode.flag_control_altitude_enabled) { - _pos_sp(2) = _offboard_control_pos_sp.z; + _pos_sp(2) = _local_pos_sp.z; } - _att_sp.yaw_body = _offboard_control_pos_sp.yaw; + _att_sp.yaw_body = _local_pos_sp.yaw; } else { /* AUTO */ -- cgit v1.2.3 From d3f182d433551c72a2fd8105481938129b8332ed Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 9 Jun 2014 11:37:33 +0200 Subject: navigator: don't check reached for land waypoints --- src/modules/navigator/mission_block.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 92090d995..861aed813 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -69,6 +69,10 @@ MissionBlock::~MissionBlock() bool MissionBlock::is_mission_item_reached() { + /* don't check landed WPs */ + if (_mission_item.nav_cmd == NAV_CMD_LAND) { + return false; + } /* TODO: count turns */ #if 0 if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || -- cgit v1.2.3 From 4ad435b483510158ea8a5b303cd680f9e982df84 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 9 Jun 2014 16:09:03 +0200 Subject: dataman: allow writing empty items with nullptr pointer to data --- src/modules/dataman/dataman.c | 4 +++- src/modules/dataman/dataman.h | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 96dc98f2a..14fee7c9a 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -324,7 +324,9 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v buffer[1] = persistence; buffer[2] = 0; buffer[3] = 0; - memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + if (count > 0) { + memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + } count += DM_SECTOR_HDR_SIZE; len = -1; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index f2b6cd4d3..b25a2a5ef 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -133,7 +133,7 @@ extern "C" { in this header, but till I find it a better home here it is */ typedef struct { - char current_offboard_waypoint_id; /* the index of the active offboard waypoint data */ + char offboard_waypoint_id; /* the index of the active offboard waypoint data */ /* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */ } persistent_system_state_t; -- cgit v1.2.3 From cad0877f67c393b698b8fc4f242944c9e1ba1bc5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 9 Jun 2014 16:10:24 +0200 Subject: mavlink: waypoint manager fixes, mission saving on reboot --- src/modules/mavlink/mavlink_main.cpp | 432 +++++++++++++++---------------- src/modules/mavlink/mavlink_main.h | 13 +- src/modules/navigator/navigator_main.cpp | 6 +- src/modules/uORB/topics/mission.h | 4 +- 4 files changed, 227 insertions(+), 228 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cb1b762a7..4e0597e80 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -954,20 +954,53 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { persistent_system_state_t sys_state; + /* init WPM state */ state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; + state->dataman_id = 0; state->current_state = MAVLINK_WPM_STATE_IDLE; state->current_partner_sysid = 0; state->current_partner_compid = 0; + state->current_dataman_id = 0; state->timestamp_lastaction = 0; state->timestamp_last_send_setpoint = 0; state->timestamp_last_send_request = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->current_dataman_id = 0; - if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { - if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1)) - state->current_dataman_id = sys_state.current_offboard_waypoint_id; + + int sys_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)); + if (sys_state_size == sizeof(sys_state)) { + if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) { + state->dataman_id = sys_state.offboard_waypoint_id; + + if (_verbose) { warnx("WPM init: using dataman ID %d", state->dataman_id); } + + /* count waypoints in current waypoints storage */ + dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + struct mission_item_s mission_item; + + int seq = 0; + while (seq < state->max_size && dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + seq++; + } + + if (_verbose) { warnx("WPM init: found %d items", seq); } + + state->size = seq; + + } else { + if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in SYSTEM_STATE", sys_state.offboard_waypoint_id); } + } + + } else { + if (_verbose) { warnx("WPM init: dataman SYSTEM_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } } + + /* init mission topic */ + mission.count = state->size; + mission.dataman_id = state->dataman_id; + mission.current_index = -1; // TODO store current index in dataman? + + publish_mission(); } /* @@ -985,7 +1018,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa); mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } + if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } } /* @@ -1009,13 +1042,12 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) mavlink_missionlib_send_message(&msg); } else if (seq == 0 && _wpm->size == 0) { - /* don't broadcast if no WPs */ } else { - mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); + if (_verbose) { warnx("WPM: Send WAYPOINT_CURRENT ERROR: seq %u out of bounds", seq); } - if (_verbose) { warnx("ERROR: index out of bounds"); } + mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); } } @@ -1031,25 +1063,15 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } + if (_verbose) { warnx("WPM: Send WAYPOINT_COUNT %u to ID %u", wpc.count, wpc.target_system); } } void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) { - + dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; struct mission_item_s mission_item; - ssize_t len = sizeof(struct mission_item_s); - dm_item_t dm_current; - - if (_wpm->current_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - if (dm_read(dm_current, seq, &mission_item, len) == len) { + if (dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { /* create mission_item_s from mavlink_mission_item_t */ mavlink_mission_item_t wp; @@ -1062,13 +1084,13 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } + if (_verbose) { warnx("WPM: Send WAYPOINT seq %u to ID %u", wp.seq, wp.target_system); } } else { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD"); - if (_verbose) { warnx("ERROR: could not read WP%u", seq); } + if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, dm_id); } } } @@ -1084,12 +1106,12 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u mavlink_missionlib_send_message(&msg); _wpm->timestamp_last_send_request = hrt_absolute_time(); - if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } + if (_verbose) { warnx("WPM: Send WAYPOINT_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } } else { mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); } + if (_verbose) { warnx("WPM: Send WAYPINT_REQUEST ERROR: seq %u exceeds list capacity", seq); } } } @@ -1110,7 +1132,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); } + if (_verbose) { warnx("WPM: Send WAYPOINT_REACHED seq %u", wp_reached.seq); } } void Mavlink::mavlink_waypoint_eventloop(uint64_t now) @@ -1120,7 +1142,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) mavlink_missionlib_send_gcs_string("Operation timeout"); - if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } + if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } _wpm->current_state = MAVLINK_WPM_STATE_IDLE; _wpm->current_partner_sysid = 0; @@ -1143,23 +1165,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); - if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { - _wpm->timestamp_lastaction = now; + if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { + _wpm->timestamp_lastaction = now; - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (_wpm->current_wp_id == _wpm->size - 1) { + if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (_wpm->current_wp_id == _wpm->size - 1) { + if (_verbose) { warnx("WPM: MISSION_ACK all items sent, switch to MAVLINK_WPM_STATE_IDLE"); } - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_wp_id = 0; + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _wpm->current_wp_id = 0; + } } - } - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); } + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } + } } - break; } @@ -1172,6 +1196,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.seq < _wpm->size) { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT OK (%d)", wpc.seq); } mission.current_index = wpc.seq; publish_mission(); @@ -1180,16 +1205,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) // mavlink_wpm_send_waypoint_current(wpc.seq); } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); } - if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); } + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); } } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - - if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); } } @@ -1205,23 +1229,22 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (_wpm->size > 0) { - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; _wpm->current_wp_id = 0; _wpm->current_partner_sysid = msg->sysid; _wpm->current_partner_compid = msg->compid; } else { - if (_verbose) { warnx("No waypoints send"); } + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST nothing to send, mission is empty"); } } _wpm->current_count = _wpm->size; mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count); } else { - mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } - if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } + mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); } } @@ -1232,88 +1255,73 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); - if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (wpr.seq >= _wpm->size) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); } - - break; - } + if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) { + _wpm->timestamp_lastaction = now; - /* - * Ensure that we are in the correct state and that the first request has id 0 - * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (wpr.seq >= _wpm->size) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: ignored, requested seq %u was out of bounds [0 %d]", wpr.seq, _wpm->size - 1); } - if (wpr.seq == 0) { - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); + break; + } - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + /* + * Ensure that we are in the correct state and that the first request has id 0 + * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + */ + if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (wpr.seq == 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - if (_verbose) { warnx("REJ. WP CMD: First id != 0"); } - - break; - } + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: first id != 0 (%d)", wpr.seq); } - } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); + break; + } - if (wpr.seq == _wpm->current_wp_id) { + } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (wpr.seq == _wpm->current_wp_id) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u (again) from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + } else if (wpr.seq == _wpm->current_wp_id + 1) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - } else if (wpr.seq == _wpm->current_wp_id + 1) { + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, should be %i or %i", wpr.seq, msg->sysid, _wpm->current_wp_id, _wpm->current_wp_id + 1); } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); + break; + } } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); } + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _wpm->current_state); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); break; } - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); } + _wpm->current_wp_id = wpr.seq; + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - break; - } + if (wpr.seq < _wpm->size) { + mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - _wpm->current_wp_id = wpr.seq; - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - if (wpr.seq < _wpm->size) { + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bounds", wpr.seq); } - mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + } } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - - if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } - } - - - } else { - //we we're target but already communicating with someone else - if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } } } - break; } @@ -1327,48 +1335,51 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); break; } if (wpc.count == 0) { - mavlink_missionlib_send_gcs_string("COUNT 0"); - - if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); } + if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } + mavlink_missionlib_send_gcs_string("WP COUNT 0: CLEAR MISSION"); break; } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - _wpm->current_count = wpc.count; - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { if (_wpm->current_wp_id == 0) { - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); + /* looks like our WAYPOINT_REQUEST was lost, try again */ + if (_verbose) { warnx("WPM: MISSION_COUNT %u (again) from ID %u", wpc.count, msg->sysid); } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } + mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _wpm->current_wp_id); } - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + break; } } else { - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _wpm->current_state); } - if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } + mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); + break; } + + _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; + _wpm->current_wp_id = 0; + _wpm->current_partner_sysid = msg->sysid; + _wpm->current_partner_compid = msg->compid; + _wpm->current_count = wpc.count; + _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } break; @@ -1378,32 +1389,28 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_msg_mission_item_decode(msg, &wp); if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { - _wpm->timestamp_lastaction = now; - /* - * ensure that we are in the correct state and that the first waypoint has id 0 + /* Ensure that we are in the correct state and that the first waypoint has id 0 * and the following waypoints have the correct ids */ - if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (wp.seq != 0) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq != 0 (%d)", wp.seq); } break; } } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (wp.seq >= _wpm->current_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq %d out of bounds [0 %d]", wp.seq, _wpm->current_count - 1); } + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); break; } if (wp.seq != _wpm->current_wp_id) { - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id); + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _wpm->current_wp_id); } + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); break; } @@ -1412,86 +1419,92 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; struct mission_item_s mission_item; - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); if (ret != OK) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } - ssize_t len = sizeof(struct mission_item_s); + dm_item_t dm_id = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - dm_item_t dm_next; + if (dm_write(dm_id, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, dm_id); } - if (_wpm->current_dataman_id == 0) { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; - mission.dataman_id = 1; - - } else { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.dataman_id = 0; - } - - if (dm_write(dm_next, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, len) != len) { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } - // offboard mission data saved correctly, now update the persistent system state - { + + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } + + _wpm->current_wp_id = wp.seq + 1; + + if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + /* got all new mission items successfully */ + if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } + + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + + /* write terminator item */ + if (_wpm->current_count < _wpm->max_size) { + if (dm_write(dm_id, _wpm->current_count, DM_PERSIST_POWER_ON_RESET, nullptr, 0) != 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR writing terminator item"); } + + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + break; + } + } + + /* offboard mission data saved correctly, now update the persistent system state */ persistent_system_state_t state; bool dm_result; - // Since we are doing a read-modify-write we must lock the item type + + /* since we are doing a read-modify-write we must lock the item type */ dm_lock(DM_KEY_SYSTEM_STATE); - // first read in the current state data. There may eventually be data other than the offboard index - // and we must preserve it - if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) { - // Not sure how to handle this? It means that either the item was never - // written, or fields have been added to the system state struct. In any case - // fields that may not be ours need to be initialized to sane values. - // For now the offboard index is the only field, so for now there - // is nothing to do here. - } - state.current_offboard_waypoint_id = mission.dataman_id; - dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); - dm_unlock(DM_KEY_SYSTEM_STATE); - if (dm_result) { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - } + /* first read in the current state data, there may eventually be data other than the offboard index + * and we must preserve it */ + ssize_t dm_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)); + if (dm_state_size != sizeof(state)) { + warnx("dataman SYSTEM_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); + } -// if (wp.current) { -// warnx("current is: %d", wp.seq); -// mission.current_index = wp.seq; -// } - // XXX ignore current set - mission.current_index = -1; + /* set new dataman storage ID */ + state.offboard_waypoint_id = _wpm->current_dataman_id; - _wpm->current_wp_id = wp.seq + 1; + /* write back to dataman */ + dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); - if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + dm_unlock(DM_KEY_SYSTEM_STATE); - if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } + if (dm_result) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman SYSTEM_STATE", state.offboard_waypoint_id); } - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); - mission.count = _wpm->current_count; + } else { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - publish_mission(); + /* update WPM state */ + _wpm->dataman_id = _wpm->current_dataman_id; + _wpm->size = _wpm->current_count; - _wpm->current_dataman_id = mission.dataman_id; - _wpm->size = _wpm->current_count; + /* update mission topic */ + mission.dataman_id = _wpm->dataman_id; + mission.count = _wpm->current_count; + mission.current_index = 0; - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + publish_mission(); + } } else { + /* request next item */ mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } @@ -1506,46 +1519,34 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - _wpm->timestamp_lastaction = now; + /* clear only active dataman storage, don't change storage id */ + dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - _wpm->size = 0; - - /* prepare mission topic */ - mission.dataman_id = -1; - mission.count = 0; - mission.current_index = -1; - publish_mission(); - - if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { - persistent_system_state_t state; - bool dm_result; - dm_lock(DM_KEY_SYSTEM_STATE); - // first read in the current state data. There may eventually be data other than the offboard index - // and we must preserve it - if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) { - // Not sure how to handle this? It means that either the item was never - // written, or fields have been added to the system state struct. In any case - // fields that may not be ours need to be initialized to sane values. - // For now the offboard index is the only field, so we can deal with it here. - } - state.current_offboard_waypoint_id = -1; - dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) == sizeof(state); - dm_unlock(DM_KEY_SYSTEM_STATE); - if (dm_result) { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - } + if (dm_clear(dm_id) == OK) { + if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } + + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + + /* update WPM state */ + _wpm->timestamp_lastaction = now; + _wpm->size = 0; + + /* update mission topic */ + mission.count = 0; + mission.current_index = -1; + + publish_mission(); } else { + if (_verbose) { warnx("WPM: CLEAR_ALL ERROR: can't clear dataman mission storage"); } + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); } - } else { mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); } + if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } } } @@ -1570,7 +1571,6 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) } - int Mavlink::mavlink_missionlib_send_gcs_string(const char *string) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 40edc4b85..fbb25029d 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -84,18 +84,19 @@ enum MAVLINK_WPM_CODES { struct mavlink_wpm_storage { - uint16_t size; - uint16_t max_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_count; + uint16_t size; ///< Count of waypoints in active mission + uint16_t max_size; ///< Maximal count of waypoints + int dataman_id; ///< Dataman storage ID for active mission + enum MAVLINK_WPM_STATES current_state; ///< Current waypoint manager state + int16_t current_wp_id; ///< Waypoint ID in current transmission + uint16_t current_count; ///< Waypoints count in current transmission uint8_t current_partner_sysid; uint8_t current_partner_compid; + int current_dataman_id; ///< Dataman storage ID for current transmission uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; uint64_t timestamp_last_send_request; uint32_t timeout; - int current_dataman_id; }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 04ea7da0e..a3426e65e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -603,14 +603,12 @@ Navigator::task_main() warnx("Could not clear geofence"); } -#if 0 // *** UNTESTED... Anton, this is for you /* Get the last offboard mission id */ persistent_system_state_t sys_state; if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { - if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1)) - _mission.set_offboard_dataman_id(sys_state.current_offboard_waypoint_id); + if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) + _mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id); } -#endif /* * do subscriptions diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index ef4bc1def..5f3cbcb1d 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -95,8 +95,8 @@ struct mission_item_s { struct mission_s { - int dataman_id; /**< default -1, there are two offboard storage places in the dataman: 0 or 1 */ - unsigned count; /**< count of the missions stored in the datamanager */ + int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ + unsigned count; /**< count of the missions stored in the dataman */ int current_index; /**< default -1, start at the one changed latest */ }; -- cgit v1.2.3 From d9b5efb263b61b8dc47ee63d7b4eb0b62853877a Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Mon, 9 Jun 2014 19:09:38 -0400 Subject: Closed additional file descriptor --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6a105762b..9d9d2a7c0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1071,6 +1071,12 @@ MulticopterPositionControl::task_main() _reset_pos_sp = true; reset_int_z = true; reset_int_xy = true; + + /* Close att_sp pub to allow offboard mode or att controller to advertise */ + if (_att_sp_pub > 0) { + close(_att_sp_pub); + _att_sp_pub = -1; + } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ -- cgit v1.2.3 From fab1b4e3669ce083c124b2ef04b38e7bdea663ed Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 10 Jun 2014 14:25:23 +0200 Subject: navigator: don't say triplet is valid in ALTCTL --- src/modules/navigator/navigator_main.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 241d1d8a4..da06f826a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -381,6 +381,14 @@ Navigator::task_main() _loiter.reset(); } + /* if nothing is running, set position setpoint triplet invalid */ + if (_navigation_mode == nullptr) { + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _update_triplet = true; + } + if (_update_triplet ) { publish_position_setpoint_triplet(); _update_triplet = false; -- cgit v1.2.3 From 3015f2e7af94e684c666689aa70c602f79810218 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 10 Jun 2014 14:32:37 +0200 Subject: mavlink: retry timeout moved to define --- src/modules/mavlink/mavlink_main.cpp | 7 ++++--- src/modules/mavlink/mavlink_main.h | 8 +++++--- src/modules/mavlink/waypoints.h | 2 +- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4e0597e80..52cae2085 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -965,7 +965,8 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->timestamp_lastaction = 0; state->timestamp_last_send_setpoint = 0; state->timestamp_last_send_request = 0; - state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; int sys_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)); if (sys_state_size == sizeof(sys_state)) { @@ -1138,7 +1139,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) void Mavlink::mavlink_waypoint_eventloop(uint64_t now) { /* check for timed-out operations */ - if (now - _wpm->timestamp_lastaction > _wpm->timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + if (now - _wpm->timestamp_lastaction > _wpm->action_timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { mavlink_missionlib_send_gcs_string("Operation timeout"); @@ -1148,7 +1149,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) _wpm->current_partner_sysid = 0; _wpm->current_partner_compid = 0; - } else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + } else if (now - _wpm->timestamp_last_send_request > _wpm->retry_timeout && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { /* try to get WP again after short timeout */ mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index fbb25029d..e31bbbb31 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -78,8 +78,9 @@ enum MAVLINK_WPM_CODES { #define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds +#define MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint #define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 @@ -96,7 +97,8 @@ struct mavlink_wpm_storage { uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; uint64_t timestamp_last_send_request; - uint32_t timeout; + uint32_t action_timeout; + uint32_t retry_timeout; }; diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index 532eff7aa..a2109f2d8 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -88,7 +88,7 @@ struct mavlink_wpm_storage { uint8_t current_partner_compid; uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; - uint32_t timeout; + uint32_t action_timeout; int current_dataman_id; }; -- cgit v1.2.3 From fd1f1c81efb384e46a5767555a58061082612c9f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 10 Jun 2014 17:24:04 +0200 Subject: navigator: added parameter for acceptance radius for take-off mission items --- src/modules/navigator/mission_block.cpp | 14 ++++++++++---- src/modules/navigator/navigator.h | 2 ++ src/modules/navigator/navigator_main.cpp | 3 ++- src/modules/navigator/navigator_params.c | 11 +++++++++++ 4 files changed, 25 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 861aed813..08576750c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -102,13 +102,19 @@ MissionBlock::is_mission_item_reached() _navigator_priv->get_global_position()->alt, &dist_xy, &dist_z); - if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - - /* require only altitude for takeoff */ - if (_navigator_priv->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) { + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) { + /* require only altitude for takeoff for multicopter */ + if (_navigator_priv->get_global_position()->alt > + altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) { + _waypoint_position_reached = true; + } + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + /* for takeoff mission items use the parameter for the takeoff acceptance radius */ + if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) { _waypoint_position_reached = true; } } else { + /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { _waypoint_position_reached = true; } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 296294a91..fe7485f56 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -109,6 +109,7 @@ public: Geofence& get_geofence() { return _geofence; } bool get_is_in_loiter() { return _is_in_loiter; } float get_loiter_radius() { return _param_loiter_radius.get(); } + float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); } int get_mavlink_fd() { return _mavlink_fd; } private: @@ -156,6 +157,7 @@ private: bool _update_triplet; /**< flags if position SP triplet needs to be published */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ + control::BlockParamFloat _param_takeoff_acceptance_radius; /**< acceptance for takeoff */ /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index da06f826a..dc8573813 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -122,7 +122,8 @@ Navigator::Navigator() : _loiter(this, "LOI"), _rtl(this, "RTL"), _update_triplet(false), - _param_loiter_radius(this, "LOITER_RAD") + _param_loiter_radius(this, "LOITER_RAD"), + _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") { updateParams(); } diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9235f3046..ca31adecc 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -53,3 +53,14 @@ * @group Mission */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); + +/** + * Takeoff Acceptance Radius (FW only) + * + * Acceptance radius for fixedwing. + * + * @unit meters + * @min 1.0 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f); -- cgit v1.2.3 From 6f24afd68a840698a9a77b46bc8f1ca81cc1f5b9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 10 Jun 2014 17:33:12 +0200 Subject: navigator: always listen to new current mission index and to new missions --- src/modules/navigator/mission.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index db606e6fa..33a1399b1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -84,6 +84,19 @@ void Mission::reset() { _first_run = true; + + /* check anyway if missions have changed so that feedback to groundstation is given */ + bool onboard_updated; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } + + bool offboard_updated; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } } bool @@ -185,6 +198,7 @@ Mission::update_offboard_mission() _offboard_mission.current_index = 0; _current_offboard_mission_index = 0; } + report_current_offboard_mission_item(); } -- cgit v1.2.3 From 5602d5dfa3033b0364a80d10aff794f2a92d62c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 23:14:57 +0200 Subject: New gyro offset based divergence detection and protection. Pending flight tests --- .../ekf_att_pos_estimator_main.cpp | 1 + .../ekf_att_pos_estimator/estimator_23states.cpp | 86 ++++++++++++++++------ .../ekf_att_pos_estimator/estimator_23states.h | 13 +++- 3 files changed, 74 insertions(+), 26 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 9cff9fab0..70c1cddb5 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1058,6 +1058,7 @@ FixedwingEstimator::task_main() rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); + rep.health_flags |= ((!(check == 4)) << 3); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 28eab49f2..29b5391ba 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2,12 +2,38 @@ #include #include +#define EKF_COVARIANCE_DIVERGED 1.0e8f + AttPosEKF::AttPosEKF() /* NOTE: DO NOT initialize class members here. Use ZeroVariables() * instead to allow clean in-air re-initialization. */ { + summedDelAng.zero(); + summedDelVel.zero(); + + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + onGround = true; + staticMode = true; + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + numericalProtection = true; + refSet = false; + storeIndex = 0; + gpsHgt = 0.0f; + baroHgt = 0.0f; + GPSstatus = 0; + VtasMeas = 0.0f; + magDeclination = 0.0f; + dAngIMU.zero(); + dVelIMU.zero(); + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); ZeroVariables(); InitialiseParameters(); @@ -2052,10 +2078,45 @@ void AttPosEKF::ForceSymmetry() { P[i][j] = 0.5f * (P[i][j] + P[j][i]); P[j][i] = P[i][j]; + + if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) || + (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) { + // XXX divergence report error + InitializeDynamic(velNED, magDeclination); + return; + } + + float symmetric = 0.5f * (P[i][j] + P[j][i]); + P[i][j] = symmetric; + P[j][i] = symmetric; } } } +bool AttPosEKF::GyroOffsetsDiverged() +{ + // Detect divergence by looking for rapid changes of the gyro offset + Vector3f current_bias; + current_bias.x = states[10]; + current_bias.y = states[11]; + current_bias.z = states[12]; + + Vector3f delta = current_bias - lastGyroOffset; + float delta_len = delta.length(); + float delta_len_scaled = 0.0f; + + // Protect against division by zero + if (delta_len > 0.0f) { + float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f); + delta_len_scaled = (5e-7 / cov_mag) * delta_len / dtIMU; + } + + bool diverged = (delta_len_scaled > 1.0f); + lastGyroOffset = current_bias; + + return diverged; +} + bool AttPosEKF::FilterHealthy() { if (!statesInitialised) { @@ -2265,7 +2326,7 @@ int AttPosEKF::CheckAndBound() } // Reset the filter if gyro offsets are excessive - if (fabs(states[10]) > 1.0f || fabsf(states[11]) > 1.0f || fabsf(states[12]) > 1.0f) { + if (GyroOffsetsDiverged()) { FillErrorReport(&last_ekf_error); InitializeDynamic(velNED, magDeclination); @@ -2408,27 +2469,7 @@ void AttPosEKF::ZeroVariables() { // Initialize on-init initialized variables - fusionModeGPS = 0; - covSkipCount = 0; - statesInitialised = false; - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - onGround = true; - staticMode = true; - useAirspeed = true; - useCompass = true; - useRangeFinder = true; - numericalProtection = true; - refSet = false; storeIndex = 0; - gpsHgt = 0.0f; - baroHgt = 0.0f; - GPSstatus = 0; - VtasMeas = 0.0f; - magDeclination = 0.0f; // Do the data structure init for (unsigned i = 0; i < n_states; i++) { @@ -2446,9 +2487,6 @@ void AttPosEKF::ZeroVariables() summedDelAng.zero(); summedDelVel.zero(); - dAngIMU.zero(); - dVelIMU.zero(); - for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 851e46371..32c514004 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -72,7 +72,7 @@ public: accelProcessNoise = 0.5f; } - struct { + struct mag_state_struct { unsigned obsIndex; float MagPred[3]; float SH_MAG[9]; @@ -88,7 +88,12 @@ public: float magZbias; float R_MAG; Mat3f DCM; - } magstate; + }; + + struct mag_state_struct magstate; + struct mag_state_struct resetMagState; + + // Global variables @@ -97,6 +102,7 @@ public: float P[n_states][n_states]; // covariance matrix float Kfusion[n_states]; // Kalman gains float states[n_states]; // state matrix + float resetStates[n_states]; float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored @@ -114,6 +120,7 @@ public: float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + Vector3f lastGyroOffset; // Last gyro offset Mat3f Tbn; // transformation matrix from body to NED coordinates Mat3f Tnb; // transformation amtrix from NED to body coordinates @@ -272,6 +279,8 @@ protected: bool FilterHealthy(); +bool GyroOffsetsDiverged(); + void ResetHeight(void); void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); -- cgit v1.2.3 From ca6463efd816a6bb01bf0ff54260e2fef33e1d84 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Tue, 10 Jun 2014 19:24:03 -0400 Subject: Modified mavlink receiver to scale offboard position and velocity messages to centimeters --- src/modules/mavlink/mavlink_receiver.cpp | 27 ++++++++++++++++++++++----- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3b3423a4b..de9a6e7ad 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -365,30 +365,47 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message case 1: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; ml_armed = true; + break; case 2: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; ml_armed = true; + break; case 3: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; ml_armed = true; + break; case 4: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; ml_armed = true; + break; default: break; } - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; + if (m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES || m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; + + } else if (m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { + + /*Temporary hack to use set_quad_swarm_roll_pitch_yaw_thrust msg for position or velocity */ + /* Converts INT16 centimeters to float meters */ + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 100.0f; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 100.0f; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 100.0f; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 100.0f; + + } if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { ml_armed = false; @@ -420,7 +437,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message loc_pos_sp.x = offboard_control_sp.p1; loc_pos_sp.y = offboard_control_sp.p2; loc_pos_sp.yaw = offboard_control_sp.p3; - loc_pos_sp.z = offboard_control_sp.p4; + loc_pos_sp.z = -offboard_control_sp.p4; /* Close fds to allow position controller to use attitude controller */ if (_att_sp_pub > 0) { -- cgit v1.2.3 From 342e08977ae5bf49c5ba941866e44fddefca4cda Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 11 Jun 2014 14:00:44 +0200 Subject: MavlinkOrbSubscription API reworked --- src/modules/mavlink/mavlink_commands.cpp | 8 +- src/modules/mavlink/mavlink_commands.h | 2 +- src/modules/mavlink/mavlink_main.cpp | 8 +- src/modules/mavlink/mavlink_messages.cpp | 311 +++++++++++++++-------- src/modules/mavlink/mavlink_orb_subscription.cpp | 38 ++- src/modules/mavlink/mavlink_orb_subscription.h | 18 +- 6 files changed, 248 insertions(+), 137 deletions(-) diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp index 5760d7512..fccd4d9a5 100644 --- a/src/modules/mavlink/mavlink_commands.cpp +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -40,21 +40,17 @@ #include "mavlink_commands.h" -MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel) +MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0) { _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); } -MavlinkCommandsStream::~MavlinkCommandsStream() -{ -} - void MavlinkCommandsStream::update(const hrt_abstime t) { struct vehicle_command_s cmd; - if (_cmd_sub->update(t, &cmd)) { + if (_cmd_sub->update(&_cmd_time, &cmd)) { /* only send commands for other systems/components */ if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { mavlink_msg_command_long_send(_channel, diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h index 6255d65df..abdba34b9 100644 --- a/src/modules/mavlink/mavlink_commands.h +++ b/src/modules/mavlink/mavlink_commands.h @@ -55,10 +55,10 @@ private: MavlinkOrbSubscription *_cmd_sub; struct vehicle_command_s *_cmd; mavlink_channel_t _channel; + uint64_t _cmd_time; public: MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); - ~MavlinkCommandsStream(); void update(const hrt_abstime t); }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 39610fdd8..046f45bd9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1900,10 +1900,12 @@ Mavlink::task_main(int argc, char *argv[]) _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); + uint64_t param_time = 0; MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); + uint64_t status_time = 0; struct vehicle_status_s status; - status_sub->update(0, &status); + status_sub->update(&status_time, &status); MavlinkCommandsStream commands_stream(this, _channel); @@ -1960,12 +1962,12 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); - if (param_sub->update(t, nullptr)) { + if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ mavlink_update_system(); } - if (status_sub->update(t, &status)) { + if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ set_hil_enabled(status.hil_state == HIL_STATE_ON); } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 184726fe8..0430987ec 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -195,9 +195,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set class MavlinkStreamHeartbeat : public MavlinkStream { public: - - ~MavlinkStreamHeartbeat() {}; - const char *get_name() const { return MavlinkStreamHeartbeat::get_name_static(); @@ -216,12 +213,8 @@ public: private: MavlinkOrbSubscription *status_sub; MavlinkOrbSubscription *pos_sp_triplet_sub; - protected: - - explicit MavlinkStreamHeartbeat() {}; - void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -233,21 +226,19 @@ protected: struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; - (void)status_sub->update(t, &status); - (void)pos_sp_triplet_sub->update(t, &pos_sp_triplet); - - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - mavlink_msg_heartbeat_send(_channel, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); + if (status_sub->update(&status) && pos_sp_triplet_sub->update(&pos_sp_triplet)) { + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + mavlink_msg_heartbeat_send(_channel, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); + } } }; @@ -255,8 +246,6 @@ protected: class MavlinkStreamSysStatus : public MavlinkStream { public: - ~MavlinkStreamSysStatus() {}; - const char *get_name() const { return MavlinkStreamSysStatus::get_name_static(); @@ -274,11 +263,8 @@ public: private: MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; protected: - explicit MavlinkStreamSysStatus() {}; - void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -287,21 +273,23 @@ protected: void send(const hrt_abstime t) { struct vehicle_status_s status; - (void)status_sub->update(t, &status); - mavlink_msg_sys_status_send(_channel, - status.onboard_control_sensors_present, - status.onboard_control_sensors_enabled, - status.onboard_control_sensors_health, - status.load * 1000.0f, - status.battery_voltage * 1000.0f, - status.battery_current * 100.0f, - status.battery_remaining * 100.0f, - status.drop_rate_comm, - status.errors_comm, - status.errors_count1, - status.errors_count2, - status.errors_count3, - status.errors_count4); + + if (status_sub->update(&status)) { + mavlink_msg_sys_status_send(_channel, + status.onboard_control_sensors_present, + status.onboard_control_sensors_enabled, + status.onboard_control_sensors_health, + status.load * 1000.0f, + status.battery_voltage * 1000.0f, + status.battery_current * 100.0f, + status.battery_remaining * 100.0f, + status.drop_rate_comm, + status.errors_comm, + status.errors_count1, + status.errors_count2, + status.errors_count3, + status.errors_count4); + } } }; @@ -309,8 +297,6 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: - ~MavlinkStreamHighresIMU() {}; - const char *get_name() const { return MavlinkStreamHighresIMU::get_name_static(); @@ -328,6 +314,7 @@ public: private: MavlinkOrbSubscription *sensor_sub; + uint64_t sensor_time; uint64_t accel_timestamp; uint64_t gyro_timestamp; @@ -335,9 +322,13 @@ private: uint64_t baro_timestamp; protected: - explicit MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0) - { - } + explicit MavlinkStreamHighresIMU() : MavlinkStream(), + sensor_time(0), + accel_timestamp(0), + gyro_timestamp(0), + mag_timestamp(0), + baro_timestamp(0) + {} void subscribe(Mavlink *mavlink) { @@ -347,7 +338,8 @@ protected: void send(const hrt_abstime t) { struct sensor_combined_s sensor; - if (sensor_sub->update(t, &sensor)) { + + if (sensor_sub->update(&sensor_time, &sensor)) { uint16_t fields_updated = 0; if (accel_timestamp != sensor.accelerometer_timestamp) { @@ -407,9 +399,13 @@ public: private: MavlinkOrbSubscription *att_sub; - struct vehicle_attitude_s *att; + uint64_t att_time; protected: + explicit MavlinkStreamAttitude() : MavlinkStream(), + att_time(0) + {} + void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); @@ -419,7 +415,7 @@ protected: { struct vehicle_attitude_s att; - if (att_sub->update(t, &att)) { + if (att_sub->update(&att_time, &att)) { mavlink_msg_attitude_send(_channel, att.timestamp / 1000, att.roll, att.pitch, att.yaw, @@ -449,8 +445,13 @@ public: private: MavlinkOrbSubscription *att_sub; + uint64_t att_time; protected: + explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), + att_time(0) + {} + void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); @@ -460,7 +461,7 @@ protected: { struct vehicle_attitude_s att; - if (att_sub->update(t, &att)) { + if (att_sub->update(&att_time, &att)) { mavlink_msg_attitude_quaternion_send(_channel, att.timestamp / 1000, att.q[0], @@ -496,21 +497,29 @@ public: private: MavlinkOrbSubscription *att_sub; - struct vehicle_attitude_s *att; + uint64_t att_time; MavlinkOrbSubscription *pos_sub; - struct vehicle_global_position_s *pos; + uint64_t pos_time; MavlinkOrbSubscription *armed_sub; - struct actuator_armed_s *armed; + uint64_t armed_time; MavlinkOrbSubscription *act_sub; - struct actuator_controls_s *act; + uint64_t act_time; MavlinkOrbSubscription *airspeed_sub; - struct airspeed_s *airspeed; + uint64_t airspeed_time; protected: + explicit MavlinkStreamVFRHUD() : MavlinkStream(), + att_time(0), + pos_time(0), + armed_time(0), + act_time(0), + airspeed_time(0) + {} + void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); @@ -528,11 +537,11 @@ protected: struct actuator_controls_s act; struct airspeed_s airspeed; - bool updated = att_sub->update(t, &att); - updated |= pos_sub->update(t, &pos); - updated |= armed_sub->update(t, &armed); - updated |= act_sub->update(t, &act); - updated |= airspeed_sub->update(t, &airspeed); + bool updated = att_sub->update(&att_time, &att); + updated |= pos_sub->update(&pos_time, &pos); + updated |= armed_sub->update(&armed_time, &armed); + updated |= act_sub->update(&act_time, &act); + updated |= airspeed_sub->update(&airspeed_time, &airspeed); if (updated) { float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); @@ -571,9 +580,13 @@ public: private: MavlinkOrbSubscription *gps_sub; - struct vehicle_gps_position_s *gps; + uint64_t gps_time; protected: + explicit MavlinkStreamGPSRawInt() : MavlinkStream(), + gps_time(0) + {} + void subscribe(Mavlink *mavlink) { gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); @@ -583,7 +596,7 @@ protected: { struct vehicle_gps_position_s gps; - if (gps_sub->update(t, &gps)) { + if (gps_sub->update(&gps_time, &gps)) { mavlink_msg_gps_raw_int_send(_channel, gps.timestamp_position, gps.fix_type, @@ -620,9 +633,17 @@ public: private: MavlinkOrbSubscription *pos_sub; + uint64_t pos_time; + MavlinkOrbSubscription *home_sub; + uint64_t home_time; protected: + explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), + pos_time(0), + home_time(0) + {} + void subscribe(Mavlink *mavlink) { pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); @@ -634,8 +655,8 @@ protected: struct vehicle_global_position_s pos; struct home_position_s home; - bool updated = pos_sub->update(t, &pos); - updated |= home_sub->update(t, &home); + bool updated = pos_sub->update(&pos_time, &pos); + updated |= home_sub->update(&home_time, &home); if (updated) { mavlink_msg_global_position_int_send(_channel, @@ -673,8 +694,13 @@ public: private: MavlinkOrbSubscription *pos_sub; + uint64_t pos_time; protected: + explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), + pos_time(0) + {} + void subscribe(Mavlink *mavlink) { pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); @@ -684,7 +710,7 @@ protected: { struct vehicle_local_position_s pos; - if (pos_sub->update(t, &pos)) { + if (pos_sub->update(&pos_time, &pos)) { mavlink_msg_local_position_ned_send(_channel, pos.timestamp / 1000, pos.x, @@ -719,8 +745,13 @@ public: private: MavlinkOrbSubscription *pos_sub; + uint64_t pos_time; protected: + explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), + pos_time(0) + {} + void subscribe(Mavlink *mavlink) { pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); @@ -730,7 +761,7 @@ protected: { struct vehicle_vicon_position_s pos; - if (pos_sub->update(t, &pos)) { + if (pos_sub->update(&pos_time, &pos)) { mavlink_msg_vicon_position_estimate_send(_channel, pos.timestamp / 1000, pos.x, @@ -777,38 +808,29 @@ protected: * the GCS does pick it up at one point */ if (home_sub->is_published()) { struct home_position_s home; - home_sub->update(t, &home); - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home.lat * 1e7), - (int32_t)(home.lon * 1e7), - (int32_t)(home.alt) * 1000.0f); + if (home_sub->update(&home)) { + mavlink_msg_gps_global_origin_send(_channel, + (int32_t)(home.lat * 1e7), + (int32_t)(home.lon * 1e7), + (int32_t)(home.alt) * 1000.0f); + } } } }; - +template class MavlinkStreamServoOutputRaw : public MavlinkStream { public: - MavlinkStreamServoOutputRaw() : MavlinkStream() - { - _instance = _n++; - } - const char *get_name() const { - return get_name_static_int(_instance); + return MavlinkStreamServoOutputRaw::get_name_static(); } static const char *get_name_static() { - return get_name_static_int(_n); - } - - static const char *get_name_static_int(unsigned n) - { - switch (n) { + switch (N) { case 0: return "SERVO_OUTPUT_RAW_0"; @@ -825,17 +847,18 @@ public: static MavlinkStream *new_instance() { - return new MavlinkStreamServoOutputRaw(); + return new MavlinkStreamServoOutputRaw(); } - static unsigned _n; - private: - MavlinkOrbSubscription *_act_sub; - - unsigned _instance; + MavlinkOrbSubscription *act_sub; + uint64_t act_time; protected: + explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), + act_time(0) + {} + void subscribe(Mavlink *mavlink) { orb_id_t act_topics[] = { @@ -845,17 +868,17 @@ protected: ORB_ID(actuator_outputs_3) }; - _act_sub = mavlink->add_orb_subscription(act_topics[_instance]); + act_sub = mavlink->add_orb_subscription(act_topics[N]); } void send(const hrt_abstime t) { struct actuator_outputs_s act; - if (_act_sub->update(t, &act)) { + if (act_sub->update(&act_time, &act)) { mavlink_msg_servo_output_raw_send(_channel, act.timestamp / 1000, - _instance, + N, act.output[0], act.output[1], act.output[2], @@ -889,10 +912,21 @@ public: private: MavlinkOrbSubscription *status_sub; + uint64_t status_time; + MavlinkOrbSubscription *pos_sp_triplet_sub; + uint64_t pos_sp_triplet_time; + MavlinkOrbSubscription *act_sub; + uint64_t act_time; protected: + explicit MavlinkStreamHILControls() : MavlinkStream(), + status_time(0), + pos_sp_triplet_time(0), + act_time(0) + {} + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -906,9 +940,9 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; struct actuator_outputs_s act; - bool updated = act_sub->update(t, &act); - (void)pos_sp_triplet_sub->update(t, &pos_sp_triplet); - (void)status_sub->update(t, &status); + bool updated = act_sub->update(&act_time, &act); + updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); + updated |= status_sub->update(&status_time, &status); if (updated && (status.arming_state == ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ @@ -1015,8 +1049,13 @@ public: private: MavlinkOrbSubscription *pos_sp_triplet_sub; + uint64_t pos_sp_triplet_time; protected: + explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), + pos_sp_triplet_time(0) + {} + void subscribe(Mavlink *mavlink) { pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); @@ -1025,7 +1064,8 @@ protected: void send(const hrt_abstime t) { struct position_setpoint_triplet_s pos_sp_triplet; - if (pos_sp_triplet_sub->update(t, &pos_sp_triplet)) { + + if (pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet)) { mavlink_msg_global_position_setpoint_int_send(_channel, MAV_FRAME_GLOBAL, (int32_t)(pos_sp_triplet.current.lat * 1e7), @@ -1057,8 +1097,13 @@ public: private: MavlinkOrbSubscription *pos_sp_sub; + uint64_t pos_sp_time; protected: + explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), + pos_sp_time(0) + {} + void subscribe(Mavlink *mavlink) { pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); @@ -1067,7 +1112,8 @@ protected: void send(const hrt_abstime t) { struct vehicle_local_position_setpoint_s pos_sp; - if (pos_sp_sub->update(t, &pos_sp)) { + + if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { mavlink_msg_local_position_setpoint_send(_channel, MAV_FRAME_LOCAL_NED, pos_sp.x, @@ -1099,8 +1145,13 @@ public: private: MavlinkOrbSubscription *att_sp_sub; + uint64_t att_sp_time; protected: + explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), + att_sp_time(0) + {} + void subscribe(Mavlink *mavlink) { att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); @@ -1109,7 +1160,8 @@ protected: void send(const hrt_abstime t) { struct vehicle_attitude_setpoint_s att_sp; - if (att_sp_sub->update(t, &att_sp)) { + + if (att_sp_sub->update(&att_sp_time, &att_sp)) { mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, att_sp.timestamp / 1000, att_sp.roll_body, @@ -1141,8 +1193,13 @@ public: private: MavlinkOrbSubscription *att_rates_sp_sub; + uint64_t att_rates_sp_time; protected: + explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), + att_rates_sp_time(0) + {} + void subscribe(Mavlink *mavlink) { att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); @@ -1151,7 +1208,8 @@ protected: void send(const hrt_abstime t) { struct vehicle_rates_setpoint_s att_rates_sp; - if (att_rates_sp_sub->update(t, &att_rates_sp)) { + + if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, att_rates_sp.timestamp / 1000, att_rates_sp.roll, @@ -1183,8 +1241,13 @@ public: private: MavlinkOrbSubscription *rc_sub; + uint64_t rc_time; protected: + explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), + rc_time(0) + {} + void subscribe(Mavlink *mavlink) { rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); @@ -1194,7 +1257,7 @@ protected: { struct rc_input_values rc; - if (rc_sub->update(t, &rc)) { + if (rc_sub->update(&rc_time, &rc)) { const unsigned port_width = 8; // Deprecated message (but still needed for compatibility!) @@ -1262,8 +1325,13 @@ public: private: MavlinkOrbSubscription *manual_sub; + uint64_t manual_time; protected: + explicit MavlinkStreamManualControl() : MavlinkStream(), + manual_time(0) + {} + void subscribe(Mavlink *mavlink) { manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); @@ -1273,7 +1341,7 @@ protected: { struct manual_control_setpoint_s manual; - if (manual_sub->update(t, &manual)) { + if (manual_sub->update(&manual_time, &manual)) { mavlink_msg_manual_control_send(_channel, mavlink_system.sysid, manual.x * 1000, @@ -1306,8 +1374,13 @@ public: private: MavlinkOrbSubscription *flow_sub; + uint64_t flow_time; protected: + explicit MavlinkStreamOpticalFlow() : MavlinkStream(), + flow_time(0) + {} + void subscribe(Mavlink *mavlink) { flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); @@ -1317,7 +1390,7 @@ protected: { struct optical_flow_s flow; - if (flow_sub->update(t, &flow)) { + if (flow_sub->update(&flow_time, &flow)) { mavlink_msg_optical_flow_send(_channel, flow.timestamp, flow.sensor_id, @@ -1349,8 +1422,13 @@ public: private: MavlinkOrbSubscription *att_ctrl_sub; + uint64_t att_ctrl_time; protected: + explicit MavlinkStreamAttitudeControls() : MavlinkStream(), + att_ctrl_time(0) + {} + void subscribe(Mavlink *mavlink) { att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); @@ -1360,7 +1438,7 @@ protected: { struct actuator_controls_s att_ctrl; - if (att_ctrl_sub->update(t, &att_ctrl)) { + if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(_channel, att_ctrl.timestamp / 1000, @@ -1402,8 +1480,13 @@ public: private: MavlinkOrbSubscription *debug_sub; + uint64_t debug_time; protected: + explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), + debug_time(0) + {} + void subscribe(Mavlink *mavlink) { debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); @@ -1413,7 +1496,7 @@ protected: { struct debug_key_value_s debug; - if (debug_sub->update(t, &debug)) { + if (debug_sub->update(&debug_time, &debug)) { /* enforce null termination */ debug.key[sizeof(debug.key) - 1] = '\0'; @@ -1455,7 +1538,7 @@ protected: void send(const hrt_abstime t) { struct vehicle_status_s status; - (void)status_sub->update(t, &status); + (void)status_sub->update(&status); if (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) { @@ -1490,8 +1573,13 @@ public: private: MavlinkOrbSubscription *range_sub; + uint64_t range_time; protected: + explicit MavlinkStreamDistanceSensor() : MavlinkStream(), + range_time(0) + {} + void subscribe(Mavlink *mavlink) { range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); @@ -1501,7 +1589,7 @@ protected: { struct range_finder_report range; - if (range_sub->update(t, &range)) { + if (range_sub->update(&range_time, &range)) { uint8_t type; @@ -1521,7 +1609,6 @@ protected: } }; -unsigned MavlinkStreamServoOutputRaw::_n = 0; StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), @@ -1534,10 +1621,10 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 0a23fb01e..901fa8f05 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -50,7 +50,6 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _fd(orb_subscribe(_topic)), _published(false), _topic(topic), - _last_check(0), next(nullptr) { } @@ -67,24 +66,39 @@ MavlinkOrbSubscription::get_topic() const } bool -MavlinkOrbSubscription::update(const hrt_abstime t, void* data) +MavlinkOrbSubscription::update(uint64_t *time, void* data) { - if (_last_check == t) { - /* already checked right now, return result of the check */ - return _updated; + // TODO this is NOT atomic operation, we can get data newer than time + // if topic was published between orb_stat and orb_copy calls. - } else { - _last_check = t; - orb_check(_fd, &_updated); + uint64_t time_topic; + if (orb_stat(_fd, &time_topic)) { + /* error getting last topic publication time */ + time_topic = 0; + } + + if (orb_copy(_topic, _fd, data)) { + /* error copying topic data */ + memset(data, 0, _topic->o_size); + return false; - if (_updated && data) { - orb_copy(_topic, _fd, data); - _published = true; + } else { + /* data copied successfully */ + _published = true; + if (time_topic != *time) { + *time = time_topic; return true; + + } else { + return false; } } +} - return false; +bool +MavlinkOrbSubscription::update(void* data) +{ + return !orb_copy(_topic, _fd, data); } bool diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index abd4031bd..71efb43af 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -53,7 +53,21 @@ public: MavlinkOrbSubscription(const orb_id_t topic); ~MavlinkOrbSubscription(); - bool update(const hrt_abstime t, void* data); + /** + * Check if subscription updated and get data. + * + * @return true only if topic was updated and data copied to buffer successfully. + * If topic was not updated since last check it will return false but still copy the data. + * If no data available data buffer will be filled with zeroes. + */ + bool update(uint64_t *time, void* data); + + /** + * Copy topic data to given buffer. + * + * @return true only if topic data copied successfully. + */ + bool update(void* data); /** * Check if the topic has been published. @@ -68,8 +82,6 @@ private: const orb_id_t _topic; ///< topic metadata int _fd; ///< subscription handle bool _published; ///< topic was ever published - hrt_abstime _last_check; ///< time of last check - bool _updated; ///< updated on last check }; -- cgit v1.2.3 From 7fa5458bc619df427fc29283ff5ff32b933f2906 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Jun 2014 19:51:33 +0200 Subject: mtecs: add D gain for speed outer loop --- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 4 ++-- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 17 +++++++++++++++++ 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index ddd6583e8..346acfaf3 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -160,8 +160,8 @@ protected: /* control blocks */ BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ - BlockPDLimited _controlAltitude; /**< P controller for altitude: output is the flight path angle setpoint */ - BlockPLimited _controlAirSpeed; /**< P controller for airspeed: output is acceleration setpoint */ + BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight path angle setpoint */ + BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */ /* Other calculation Blocks */ control::BlockDerivative _airspeedDerivative; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 39514b223..6165611b9 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -233,6 +233,23 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); */ PARAM_DEFINE_FLOAT(MT_ACC_P, 1.5f); +/** + * D gain for the airspeed control + * Maps the change of airspeed error to the acceleration setpoint + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f); + +/** + * Lowpass for ACC error derivative calculation (see MT_ACC_D) + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f); + /** * Minimal acceleration (air) * -- cgit v1.2.3 From a1bcd5d3136fb5320996c2df90eea91d230d55ac Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Jun 2014 20:22:37 +0200 Subject: mtecs: small cleanup, move subclass to own file --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/fw_pos_control_l1/module.mk | 1 + .../fw_pos_control_l1/mtecs/limitoverride.cpp | 71 ++++++++++++++ .../fw_pos_control_l1/mtecs/limitoverride.h | 107 +++++++++++++++++++++ src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 25 ----- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 57 +---------- 6 files changed, 181 insertions(+), 82 deletions(-) create mode 100644 src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp create mode 100644 src/modules/fw_pos_control_l1/mtecs/limitoverride.h diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index fe4eb66a9..5df17e2ec 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1456,7 +1456,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ if (ground_speed_length > FLT_EPSILON) { flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } - fwPosctrl::mTecs::LimitOverride limitOverride; + fwPosctrl::LimitOverride limitOverride; if (climbout_mode) { limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); } else { diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index c887223f4..af155fe08 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -41,4 +41,5 @@ SRCS = fw_pos_control_l1_main.cpp \ fw_pos_control_l1_params.c \ landingslope.cpp \ mtecs/mTecs.cpp \ + mtecs/limitoverride.cpp \ mtecs/mTecs_params.c diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp new file mode 100644 index 000000000..58795edb6 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file limitoverride.cpp + * + * @author Thomas Gubler + */ + +#include "limitoverride.h" + +namespace fwPosctrl { + +bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle, + BlockOutputLimiter &outputLimiterPitch) +{ + bool ret = false; + + if (overrideThrottleMinEnabled) { + outputLimiterThrottle.setMin(overrideThrottleMin); + ret = true; + } + if (overrideThrottleMaxEnabled) { + outputLimiterThrottle.setMax(overrideThrottleMax); + ret = true; + } + if (overridePitchMinEnabled) { + outputLimiterPitch.setMin(overridePitchMin); + ret = true; + } + if (overridePitchMaxEnabled) { + outputLimiterPitch.setMax(overridePitchMax); + ret = true; + } + + return ret; +} + +} /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h new file mode 100644 index 000000000..64c2e7bbd --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file limitoverride.h + * + * @author Thomas Gubler + */ + + +#ifndef LIMITOVERRIDE_H_ +#define LIMITOVERRIDE_H_ + +#include "mTecs_blocks.h" + +namespace fwPosctrl +{ + +/* A small class which provides helper functions to override control output limits which are usually set by +* parameters in special cases +*/ +class LimitOverride +{ +public: + LimitOverride() : + overrideThrottleMinEnabled(false), + overrideThrottleMaxEnabled(false), + overridePitchMinEnabled(false), + overridePitchMaxEnabled(false) + {}; + + ~LimitOverride() {}; + + /* + * Override the limits of the outputlimiter instances given by the arguments with the limits saved in + * this class (if enabled) + * @return true if the limit was applied + */ + bool applyOverride(BlockOutputLimiter &outputLimiterThrottle, + BlockOutputLimiter &outputLimiterPitch); + + /* Functions to enable or disable the override */ + void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled, + &overrideThrottleMin, value); } + void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); } + void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled, + &overrideThrottleMax, value); } + void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); } + void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled, + &overridePitchMin, value); } + void disablePitchMinOverride() { disable(&overridePitchMinEnabled); } + void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled, + &overridePitchMax, value); } + void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); } + +protected: + bool overrideThrottleMinEnabled; + float overrideThrottleMin; + bool overrideThrottleMaxEnabled; + float overrideThrottleMax; + bool overridePitchMinEnabled; + float overridePitchMin; //in degrees (replaces param values) + bool overridePitchMaxEnabled; + float overridePitchMax; //in degrees (replaces param values) + + /* Enable a specific limit override */ + void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; }; + + /* Disable a specific limit override */ + void disable(bool *flag) { *flag = false; }; +}; + +} /* namespace fwPosctrl */ + +#endif /* LIMITOVERRIDE_H_ */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 32f9f19ca..5894333f3 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -302,29 +302,4 @@ void mTecs::debug(const char *fmt, ...) { debug_print(fmt, args); } -bool mTecs::LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle, - BlockOutputLimiter &outputLimiterPitch) -{ - bool ret = false; - - if (overrideThrottleMinEnabled) { - outputLimiterThrottle.setMin(overrideThrottleMin); - ret = true; - } - if (overrideThrottleMaxEnabled) { - outputLimiterThrottle.setMax(overrideThrottleMax); - ret = true; - } - if (overridePitchMinEnabled) { - outputLimiterPitch.setMin(overridePitchMin); - ret = true; - } - if (overridePitchMaxEnabled) { - outputLimiterPitch.setMax(overridePitchMax); - ret = true; - } - - return ret; -} - } /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 346acfaf3..0369640f2 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -44,6 +44,7 @@ #define MTECS_H_ #include "mTecs_blocks.h" +#include "limitoverride.h" #include #include @@ -60,62 +61,6 @@ public: mTecs(); virtual ~mTecs(); - /* A small class which provides helper fucntions to override control output limits which are usually set by - * parameters in special cases - */ - class LimitOverride - { - public: - LimitOverride() : - overrideThrottleMinEnabled(false), - overrideThrottleMaxEnabled(false), - overridePitchMinEnabled(false), - overridePitchMaxEnabled(false) - {}; - - ~LimitOverride() {}; - - /* - * Override the limits of the outputlimiter instances given by the arguments with the limits saved in - * this class (if enabled) - * @return true if the limit was applied - */ - bool applyOverride(BlockOutputLimiter &outputLimiterThrottle, - BlockOutputLimiter &outputLimiterPitch); - - /* Functions to enable or disable the override */ - void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled, - &overrideThrottleMin, value); } - void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); } - void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled, - &overrideThrottleMax, value); } - void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); } - void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled, - &overridePitchMin, value); } - void disablePitchMinOverride() { disable(&overridePitchMinEnabled); } - void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled, - &overridePitchMax, value); } - void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); } - - protected: - bool overrideThrottleMinEnabled; - float overrideThrottleMin; - bool overrideThrottleMaxEnabled; - float overrideThrottleMax; - bool overridePitchMinEnabled; - float overridePitchMin; //in degrees (replaces param values) - bool overridePitchMaxEnabled; - float overridePitchMax; //in degrees (replaces param values) - - /* Enable a specific limit override */ - void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; - }; - /* Disable a specific limit override */ - void disable(bool *flag) { *flag = false; }; - - - }; - /* * Control in altitude setpoint and speed mode */ -- cgit v1.2.3 From f9946c98a809d18e0a037ee45f39195fd92c62fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Jun 2014 21:03:27 +0200 Subject: mtecs: filter airspeed --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 22 +++++++++++++++------- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 12 +++++++----- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 6 ++++++ src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- src/modules/uORB/topics/tecs_status.h | 1 + 6 files changed, 32 insertions(+), 13 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 5894333f3..03353cbc1 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -58,6 +58,7 @@ mTecs::mTecs() : _controlEnergyDistribution(this, "PIT", true), _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), + _airspeedLowpass(this, "A_LP"), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), _pitchSp(0.0f), @@ -122,12 +123,18 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* time measurement */ updateTimeMeasurement(); + /* Filter arispeed */ + float airspeedFiltered = _airspeedLowpass.update(airspeed); + /* calculate longitudinal acceleration setpoint from airspeed setpoint*/ - float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); + float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered); /* Debug output */ if (_counter % 10 == 0) { - debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f," + "accelerationLongitudinalSp%.4f", + (double)airspeedSp, (double)airspeed, + (double)airspeedFiltered, (double)accelerationLongitudinalSp); } /* Write part of the status message */ @@ -135,19 +142,20 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng _status.flightPathAngle = flightPathAngle; _status.airspeedSp = airspeedSp; _status.airspeed = airspeed; + _status.airspeedFiltered = airspeedFiltered; /* use longitudinal acceleration setpoint for total energy control */ - return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, + return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered, accelerationLongitudinalSp, mode, limitOverride); } -int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, +int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || - !isfinite(airspeed) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { + !isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { return -1; } /* time measurement */ @@ -160,7 +168,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight float flightPathAngleError = flightPathAngleSp - flightPathAngle; float airspeedDerivative = 0.0f; if(_airspeedDerivative.getDt() > 0.0f) { - airspeedDerivative = _airspeedDerivative.update(airspeed); + airspeedDerivative = _airspeedDerivative.update(airspeedFiltered); } float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; float airspeedDerivativeSp = accelerationLongitudinalSp; @@ -186,7 +194,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight } /* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */ - if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeed < _airspeedMin.get()) { + if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) { mode = TECS_MODE_UNDERSPEED; } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 0369640f2..c102f5dee 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -76,7 +76,7 @@ public: /* * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) */ - int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, + int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride); /* @@ -90,9 +90,10 @@ public: void resetDerivatives(float airspeed); /* Accessors */ - bool getEnabled() {return _mTecsEnabled.get() > 0;} - float getThrottleSetpoint() {return _throttleSp;} - float getPitchSetpoint() {return _pitchSp;} + bool getEnabled() { return _mTecsEnabled.get() > 0; } + float getThrottleSetpoint() { return _throttleSp; } + float getPitchSetpoint() { return _pitchSp; } + float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); } protected: /* parameters */ @@ -109,7 +110,8 @@ protected: BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */ /* Other calculation Blocks */ - control::BlockDerivative _airspeedDerivative; + control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ + control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ /* Output setpoints */ float _throttleSp; /**< Throttle Setpoint from 0 to 1 */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 6165611b9..c95bf1dc9 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -222,6 +222,12 @@ PARAM_DEFINE_FLOAT(MT_FPA_MIN, -10.0f); */ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); +/** + * Lowpass (cutoff freq.) for airspeed + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f); /** * P gain for the airspeed control diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c19579f0f..0813bf7b0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1509,6 +1509,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; + log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a874351b3..c42ff0afe 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -355,6 +355,7 @@ struct log_TECS_s { float flightPathAngle; float airspeedSp; float airspeed; + float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; @@ -430,7 +431,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "ffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), + LOG_FORMAT(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index fc530b295..05310e906 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -70,6 +70,7 @@ struct tecs_status_s { float flightPathAngle; float airspeedSp; float airspeed; + float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; -- cgit v1.2.3 From d9a64bb58720300417f190b8a8b610ab2966a11f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 11 Jun 2014 23:16:22 +0200 Subject: navigator: don't give up after DO_JUMPS --- src/modules/navigator/mission.cpp | 43 +++++++++++++++++++++------------------ 1 file changed, 23 insertions(+), 20 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 33a1399b1..a551df9a2 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -391,29 +391,32 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { + /* do DO_JUMP as many times as requested */ + if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + + /* only raise the repeat count if this is for the current mission item + * but not for the next mission item */ + if (is_current) { + (new_mission_item->do_jump_current_count)++; + /* save repeat count */ + if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, + new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the + * dataman */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP waypoint could not be written"); + return false; + } + } + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; + } else { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: DO JUMP repetitions completed"); - return false; - } - - /* only raise the repeat count if this is for the current mission item - * but not for the next mission item */ - if (is_current) { - (new_mission_item->do_jump_current_count)++; - - /* save repeat count */ - if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, - new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the dataman */ - mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP waypoint could not be written"); - return false; - } + /* no more DO_JUMPS, therefore just try to continue with next mission item */ + (*mission_index)++; } - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; } else { /* if it's not a DO_JUMP, then we were successful */ -- cgit v1.2.3 From 44481e3773b7a576b31727c64931216112d953e0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 12 Jun 2014 12:01:54 +0200 Subject: mavlink: sign of climb rate fixed in VFR_HUD message --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e0a148b53..aff1aa929 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -554,7 +554,7 @@ protected: heading, throttle, pos.alt, - pos.vel_d); + -pos.vel_d); } } }; -- cgit v1.2.3 From 2951962c2104a0b2a284a7c5208171b257ed9734 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 12 Jun 2014 13:11:45 +0200 Subject: dataman: rename SYSTEM_STATE section to MISSION_STATE --- src/modules/dataman/dataman.c | 6 +++--- src/modules/dataman/dataman.h | 4 ++-- src/modules/mavlink/mavlink_main.cpp | 18 +++++++++--------- src/modules/navigator/navigator_main.cpp | 2 +- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 14fee7c9a..4a89c7d41 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -116,7 +116,7 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, DM_KEY_WAYPOINTS_ONBOARD_MAX, - DM_KEY_SYSTEM_STATE_MAX + DM_KEY_MISSION_STATE_MAX }; /* Table of offset for index 0 of each item type */ @@ -642,11 +642,11 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < dm_number_of_funcs; i++) g_func_counts[i] = 0; - /* Initialize the item type locks, for now only DM_KEY_SYSTEM_STATE supports locking */ + /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */ sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */ for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) g_item_locks[i] = NULL; - g_item_locks[DM_KEY_SYSTEM_STATE] = &g_sys_state_mutex; + g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex; g_task_should_exit = false; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index b25a2a5ef..215ec4c76 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -53,7 +53,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ - DM_KEY_SYSTEM_STATE, /* Persistent system state storage */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -64,7 +64,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_SYSTEM_STATE_MAX = 1 + DM_KEY_MISSION_STATE_MAX = 1 }; /** Data persistence levels */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9159f5d61..be1fc50a6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -969,7 +969,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - int sys_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)); + int sys_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)); if (sys_state_size == sizeof(sys_state)) { if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) { state->dataman_id = sys_state.offboard_waypoint_id; @@ -990,11 +990,11 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->size = seq; } else { - if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in SYSTEM_STATE", sys_state.offboard_waypoint_id); } + if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in MISSION_STATE", sys_state.offboard_waypoint_id); } } } else { - if (_verbose) { warnx("WPM init: dataman SYSTEM_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } + if (_verbose) { warnx("WPM init: dataman MISSION_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } } /* init mission topic */ @@ -1467,25 +1467,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) bool dm_result; /* since we are doing a read-modify-write we must lock the item type */ - dm_lock(DM_KEY_SYSTEM_STATE); + dm_lock(DM_KEY_MISSION_STATE); /* first read in the current state data, there may eventually be data other than the offboard index * and we must preserve it */ - ssize_t dm_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)); + ssize_t dm_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &state, sizeof(state)); if (dm_state_size != sizeof(state)) { - warnx("dataman SYSTEM_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); + warnx("dataman MISSION_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); } /* set new dataman storage ID */ state.offboard_waypoint_id = _wpm->current_dataman_id; /* write back to dataman */ - dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); + dm_result = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); - dm_unlock(DM_KEY_SYSTEM_STATE); + dm_unlock(DM_KEY_MISSION_STATE); if (dm_result) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman SYSTEM_STATE", state.offboard_waypoint_id); } + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman MISSION_STATE", state.offboard_waypoint_id); } mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a3426e65e..c7b39ba93 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -605,7 +605,7 @@ Navigator::task_main() /* Get the last offboard mission id */ persistent_system_state_t sys_state; - if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { + if (dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) _mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id); } -- cgit v1.2.3 From 7983e105bf1aa6b8cf13ed49dac36c4f1b3a034f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Jun 2014 15:28:21 +0200 Subject: Much more aggressive reset logic bounding the filter effectively --- .../ekf_att_pos_estimator/estimator_23states.cpp | 64 +++++++++++++++++++--- .../ekf_att_pos_estimator/estimator_23states.h | 8 +++ .../ekf_att_pos_estimator/estimator_utilities.h | 8 +-- 3 files changed, 69 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 29b5391ba..de3c9d60e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1,5 +1,6 @@ #include "estimator_23states.h" #include +#include #include #define EKF_COVARIANCE_DIVERGED 1.0e8f @@ -33,6 +34,8 @@ AttPosEKF::AttPosEKF() magDeclination = 0.0f; dAngIMU.zero(); dVelIMU.zero(); + ekfDiverged = false; + delAngTotal.zero(); memset(&last_ekf_error, 0, sizeof(last_ekf_error)); ZeroVariables(); @@ -70,6 +73,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED() dVelIMU.y = dVelIMU.y; dVelIMU.z = dVelIMU.z - states[13]; + delAngTotal.x += correctedDelAng.x; + delAngTotal.y += correctedDelAng.y; + delAngTotal.z += correctedDelAng.z; + // Save current measurements Vector3f prevDelAng = correctedDelAng; @@ -201,7 +208,8 @@ void AttPosEKF::CovariancePrediction(float dt) float nextP[n_states][n_states]; // calculate covariance prediction process noise - for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f; for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; // scale gyro bias noise when on ground to allow for faster bias estimation for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; @@ -2081,7 +2089,7 @@ void AttPosEKF::ForceSymmetry() if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) || (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) { - // XXX divergence report error + last_ekf_error.covariancesExcessive = true; InitializeDynamic(velNED, magDeclination); return; } @@ -2117,6 +2125,24 @@ bool AttPosEKF::GyroOffsetsDiverged() return diverged; } +bool AttPosEKF::VelNEDDiverged() +{ + Vector3f current_vel; + current_vel.x = states[4]; + current_vel.y = states[5]; + current_vel.z = states[6]; + + Vector3f gps_vel; + gps_vel.x = velNED[0]; + gps_vel.y = velNED[1]; + gps_vel.z = velNED[2]; + + Vector3f delta = current_vel - gps_vel; + float delta_len = delta.length(); + + return (delta_len > 8.0f); +} + bool AttPosEKF::FilterHealthy() { if (!statesInitialised) { @@ -2188,6 +2214,7 @@ void AttPosEKF::FillErrorReport(struct ekf_status_report *err) { err->states[i] = states[i]; } + err->n_states = n_states; err->velHealth = current_ekf_state.velHealth; err->posHealth = current_ekf_state.posHealth; @@ -2290,13 +2317,19 @@ int AttPosEKF::CheckAndBound() // Store the old filter state bool currStaticMode = staticMode; + if (ekfDiverged) { + ekfDiverged = false; + } + + int ret = 0; + // Reset the filter if the states went NaN if (StatesNaN(&last_ekf_error)) { ekf_debug("re-initializing dynamic"); InitializeDynamic(velNED, magDeclination); - return 1; + ret = 1; } // Reset the filter if the IMU data is too old @@ -2308,7 +2341,7 @@ int AttPosEKF::CheckAndBound() ResetStoredStates(); // that's all we can do here, return - return 2; + ret = 2; } // Check if we're on ground - this also sets static mode. @@ -2322,7 +2355,7 @@ int AttPosEKF::CheckAndBound() ResetHeight(); ResetStoredStates(); - return 3; + ret = 3; } // Reset the filter if gyro offsets are excessive @@ -2331,10 +2364,23 @@ int AttPosEKF::CheckAndBound() InitializeDynamic(velNED, magDeclination); // that's all we can do here, return - return 4; + ret = 4; } - return 0; + // Reset the filter if it diverges too far from GPS + if (VelNEDDiverged()) { + FillErrorReport(&last_ekf_error); + InitializeDynamic(velNED, magDeclination); + + // that's all we can do here, return + ret = 5; + } + + if (ret) { + ekfDiverged = true; + } + + return ret; } void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) @@ -2386,6 +2432,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { + ZeroVariables(); + // Fill variables with valid data velNED[0] = initvelNED[0]; velNED[1] = initvelNED[1]; @@ -2469,6 +2517,7 @@ void AttPosEKF::ZeroVariables() { // Initialize on-init initialized variables + storeIndex = 0; // Do the data structure init @@ -2486,6 +2535,7 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + lastGyroOffset.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 32c514004..1bf1312b0 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -2,6 +2,9 @@ #include "estimator_utilities.h" +const unsigned int n_states = 23; +const unsigned int data_buffer_size = 50; + class AttPosEKF { public: @@ -121,6 +124,7 @@ public: Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) Vector3f lastGyroOffset; // Last gyro offset + Vector3f delAngTotal; Mat3f Tbn; // transformation matrix from body to NED coordinates Mat3f Tnb; // transformation amtrix from NED to body coordinates @@ -180,6 +184,8 @@ public: bool useCompass; ///< boolean true if magnetometer data is being used bool useRangeFinder; ///< true when rangefinder is being used + bool ekfDiverged; + struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; @@ -281,6 +287,8 @@ bool FilterHealthy(); bool GyroOffsetsDiverged(); +bool VelNEDDiverged(); + void ResetHeight(void); void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 714dfe623..e5f76d7cd 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -46,9 +46,6 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1); void swap_var(float &d1, float &d2); -const unsigned int n_states = 23; -const unsigned int data_buffer_size = 50; - enum GPS_FIX { GPS_FIX_NOFIX = 0, GPS_FIX_2D = 2, @@ -65,7 +62,8 @@ struct ekf_status_report { uint32_t velFailTime; uint32_t posFailTime; uint32_t hgtFailTime; - float states[n_states]; + float states[32]; + unsigned n_states; bool angNaN; bool summedDelVelNaN; bool KHNaN; @@ -74,6 +72,8 @@ struct ekf_status_report { bool covarianceNaN; bool kalmanGainsNaN; bool statesNaN; + bool gyroOffsetsExcessive; + bool covariancesExcessive; }; void ekf_debug(const char *fmt, ...); \ No newline at end of file -- cgit v1.2.3 From 3685ec5c975bf5a7f39f726fe07b99f1c26350f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Jun 2014 15:52:15 +0200 Subject: Loosen velocity threshold to 20 m/s to only catch the really bad instances and let the system live in peace else --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index de3c9d60e..352d9a0ce 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2140,7 +2140,7 @@ bool AttPosEKF::VelNEDDiverged() Vector3f delta = current_vel - gps_vel; float delta_len = delta.length(); - return (delta_len > 8.0f); + return (delta_len > 20.0f); } bool AttPosEKF::FilterHealthy() -- cgit v1.2.3 From a419a2ebf37ba0fafd0b95732a0992d3d7425689 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Jun 2014 15:57:13 +0200 Subject: Report excessive covariances --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 352d9a0ce..c2282fbb9 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2376,6 +2376,12 @@ int AttPosEKF::CheckAndBound() ret = 5; } + if (current_ekf_state.covariancesExcessive) { + FillErrorReport(&last_ekf_error); + current_ekf_state.covariancesExcessive = false; + ret = 6; + } + if (ret) { ekfDiverged = true; } -- cgit v1.2.3 From d48a8bc073a3aa5f515c582a1c2c3cae58a8d783 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Jun 2014 19:09:18 +0200 Subject: navigator: renamed the different RTL states --- src/modules/commander/commander.cpp | 10 +++---- src/modules/commander/state_machine_helper.cpp | 8 +++--- src/modules/mavlink/mavlink_messages.cpp | 4 +-- src/modules/navigator/navigator_main.cpp | 4 +-- src/modules/uORB/topics/vehicle_status.h | 37 +++++++++++++------------- 5 files changed, 32 insertions(+), 31 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb75b2af0..e992706ac 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1300,7 +1300,7 @@ int commander_thread_main(int argc, char *argv[]) /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; + status.failsafe_state = FAILSAFE_STATE_RC_LOSS; } else { status.failsafe_state = FAILSAFE_STATE_LAND; } @@ -1313,10 +1313,10 @@ int commander_thread_main(int argc, char *argv[]) /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RTL_RC) { + mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; + status.failsafe_state = FAILSAFE_STATE_RC_LOSS; mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); } else { /* this probably doesn't make sense since we are in mission and have global position */ @@ -1719,8 +1719,8 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 214480079..c52e618ef 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -406,12 +406,12 @@ void set_nav_state(struct vehicle_status_s *status) } break; - case FAILSAFE_STATE_RTL_RC: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC; + case FAILSAFE_STATE_RC_LOSS: + status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS; break; - case FAILSAFE_STATE_RTL_DL: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL; + case FAILSAFE_STATE_DL_LOSS: + status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS; break; case FAILSAFE_STATE_LAND: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e7d2ba515..b58fb4cb8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -161,8 +161,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dc8573813..20cff5c28 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,8 +351,8 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: _navigation_mode = &_rtl; break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 259d7329e..9390ff717 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Petri Tanskanen - * @author Thomas Gubler - * @author Julian Oes + * Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,6 +41,11 @@ * All apps should write to subsystem_info: * * (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app) + * + * @author Lorenz Meier + * @author Petri Tanskanen + * @author Thomas Gubler + * @author Julian Oes */ #ifndef VEHICLE_STATUS_H_ @@ -90,25 +91,25 @@ typedef enum { typedef enum { FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RTL_RC, /**< Return To Launch on remote control loss */ - FAILSAFE_STATE_RTL_DL, /**< Return To Launch on datalink loss */ + FAILSAFE_STATE_RC_LOSS, /**< Return To Launch on remote control loss */ + FAILSAFE_STATE_DL_LOSS, /**< Return To Launch on datalink loss */ FAILSAFE_STATE_LAND, /**< Land as safe as possible */ FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ FAILSAFE_STATE_MAX, } failsafe_state_t; typedef enum { - NAVIGATION_STATE_MANUAL = 0, - NAVIGATION_STATE_ACRO, - NAVIGATION_STATE_ALTCTL, - NAVIGATION_STATE_POSCTL, - NAVIGATION_STATE_AUTO_MISSION, - NAVIGATION_STATE_AUTO_LOITER, - NAVIGATION_STATE_AUTO_RTL, - NAVIGATION_STATE_AUTO_RTL_RC, - NAVIGATION_STATE_AUTO_RTL_DL, - NAVIGATION_STATE_LAND, - NAVIGATION_STATE_TERMINATION, + NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ + NAVIGATION_STATE_ACRO, /**< Acro mode */ + NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */ + NAVIGATION_STATE_POSCTL, /**< Position control mode */ + NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ + NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ + NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ + NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS, /**< Auto failsafe on RC loss mode */ + NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS, /**< Auto failsafe on datalink loss mode */ + NAVIGATION_STATE_LAND, /**< Land mode */ + NAVIGATION_STATE_TERMINATION, /**< Termination mode */ } navigation_state_t; enum VEHICLE_MODE_FLAG { -- cgit v1.2.3 From 3b39a8a789e347e318375ebe18c583eabec0501c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Jun 2014 19:37:25 +0200 Subject: navigator: rename update and reset calls to on_active and on_inactive --- src/modules/navigator/mission.cpp | 6 +++--- src/modules/navigator/mission.h | 4 ++-- src/modules/navigator/navigator_main.cpp | 12 ++++++------ src/modules/navigator/navigator_mode.cpp | 6 +++--- src/modules/navigator/navigator_mode.h | 4 ++-- src/modules/navigator/rtl.cpp | 6 +++--- src/modules/navigator/rtl.h | 4 ++-- 7 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a551df9a2..9244063b1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -72,7 +72,7 @@ Mission::Mission(Navigator *navigator, const char *name) : /* load initial params */ updateParams(); /* set initial mission items */ - reset(); + on_inactive(); } @@ -81,7 +81,7 @@ Mission::~Mission() } void -Mission::reset() +Mission::on_inactive() { _first_run = true; @@ -100,7 +100,7 @@ Mission::reset() } bool -Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) +Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { bool updated = false; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index a3dd09ecd..322aaf96a 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -78,12 +78,12 @@ public: /** * This function is called while the mode is inactive */ - virtual void reset(); + virtual void on_inactive(); /** * This function is called while the mode is active */ - virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); private: /** diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 20cff5c28..a3c190c7f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -365,21 +365,21 @@ Navigator::task_main() /* TODO: make list of modes and loop through it */ if (_navigation_mode == &_mission) { - _update_triplet = _mission.update(&_pos_sp_triplet); + _update_triplet = _mission.on_active(&_pos_sp_triplet); } else { - _mission.reset(); + _mission.on_inactive(); } if (_navigation_mode == &_rtl) { - _update_triplet = _rtl.update(&_pos_sp_triplet); + _update_triplet = _rtl.on_active(&_pos_sp_triplet); } else { - _rtl.reset(); + _rtl.on_inactive(); } if (_navigation_mode == &_loiter) { - _update_triplet = _loiter.update(&_pos_sp_triplet); + _update_triplet = _loiter.on_active(&_pos_sp_triplet); } else { - _loiter.reset(); + _loiter.on_inactive(); } /* if nothing is running, set position setpoint triplet invalid */ diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index c96829190..25e767c2b 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -48,7 +48,7 @@ NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : /* load initial params */ updateParams(); /* set initial mission items */ - reset(); + on_inactive(); } NavigatorMode::~NavigatorMode() @@ -56,13 +56,13 @@ NavigatorMode::~NavigatorMode() } void -NavigatorMode::reset() +NavigatorMode::on_inactive() { _first_run = true; } bool -NavigatorMode::update(struct position_setpoint_triplet_s *pos_sp_triplet) +NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { pos_sp_triplet->current.valid = false; _first_run = false; diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 0844bb94d..cbb53d91b 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -68,7 +68,7 @@ public: /** * This function is called while the mode is inactive */ - virtual void reset(); + virtual void on_inactive(); /** * This function is called while the mode is active @@ -76,7 +76,7 @@ public: * @param position setpoint triplet to set * @return true if position setpoint triplet has been changed */ - virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); protected: Navigator *_navigator; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 8888c6b62..c1b1d3f09 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -64,7 +64,7 @@ RTL::RTL(Navigator *navigator, const char *name) : /* load initial params */ updateParams(); /* initial reset */ - reset(); + on_inactive(); } RTL::~RTL() @@ -72,14 +72,14 @@ RTL::~RTL() } void -RTL::reset() +RTL::on_inactive() { _first_run = true; _rtl_state = RTL_STATE_NONE; } bool -RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet) +RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { bool updated = false; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9836f5064..bcccf7454 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -70,7 +70,7 @@ public: /** * This function is called while the mode is inactive */ - void reset(); + void on_inactive(); /** * This function is called while the mode is active @@ -78,7 +78,7 @@ public: * @param position setpoint triplet that needs to be set * @return true if updated */ - bool update(position_setpoint_triplet_s *pos_sp_triplet); + bool on_active(position_setpoint_triplet_s *pos_sp_triplet); private: -- cgit v1.2.3 From 9bb8b12f43de3aa4e7f24c516e7e8e7e9e6c196d Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Thu, 12 Jun 2014 15:50:06 -0700 Subject: Using a vector to store navigation modes --- src/modules/navigator/navigator.h | 4 ++++ src/modules/navigator/navigator_main.cpp | 29 ++++++++++++----------------- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index fe7485f56..dfa939c38 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -40,6 +40,8 @@ #ifndef NAVIGATOR_H #define NAVIGATOR_H +#include + #include #include @@ -153,6 +155,8 @@ private: Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ + std::vector _navigation_mode_vector; + bool _is_in_loiter; /**< flags if current position SP can be used to loiter */ bool _update_triplet; /**< flags if position SP triplet needs to be published */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a3c190c7f..ac32d4b22 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -125,6 +125,11 @@ Navigator::Navigator() : _param_loiter_radius(this, "LOITER_RAD"), _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") { + /* Create a list of our possible navigation types */ + _navigation_mode_vector.push_back(&_mission); + _navigation_mode_vector.push_back(&_loiter); + _navigation_mode_vector.push_back(&_rtl); + updateParams(); } @@ -363,23 +368,13 @@ Navigator::task_main() break; } - /* TODO: make list of modes and loop through it */ - if (_navigation_mode == &_mission) { - _update_triplet = _mission.on_active(&_pos_sp_triplet); - } else { - _mission.on_inactive(); - } - - if (_navigation_mode == &_rtl) { - _update_triplet = _rtl.on_active(&_pos_sp_triplet); - } else { - _rtl.on_inactive(); - } - - if (_navigation_mode == &_loiter) { - _update_triplet = _loiter.on_active(&_pos_sp_triplet); - } else { - _loiter.on_inactive(); + /* iterate through navigation modes and set active/inactive for each */ + for(unsigned int i = 0; i < _navigation_mode_vector.size(); i++) { + if (_navigation_mode == _navigation_mode_vector[i]) { + _update_triplet = _navigation_mode_vector[i]->on_active(&_pos_sp_triplet); + } else { + _navigation_mode_vector[i]->on_inactive(); + } } /* if nothing is running, set position setpoint triplet invalid */ -- cgit v1.2.3 From e53c2ab98504cf398a48f5051383796a1ad4b85e Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Thu, 12 Jun 2014 16:22:04 -0700 Subject: Switched to using c-type arrays --- src/modules/navigator/navigator.h | 6 +++--- src/modules/navigator/navigator_main.cpp | 14 +++++++------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index dfa939c38..929de0259 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -40,8 +40,6 @@ #ifndef NAVIGATOR_H #define NAVIGATOR_H -#include - #include #include @@ -60,6 +58,8 @@ #include "rtl.h" #include "geofence.h" +#define NAVIGATOR_MODE_ARRAY_SIZE 3 + class Navigator : public control::SuperBlock { public: @@ -155,7 +155,7 @@ private: Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ - std::vector _navigation_mode_vector; + NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ bool _is_in_loiter; /**< flags if current position SP can be used to loiter */ bool _update_triplet; /**< flags if position SP triplet needs to be published */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ac32d4b22..b91af8e4c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -126,9 +126,9 @@ Navigator::Navigator() : _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") { /* Create a list of our possible navigation types */ - _navigation_mode_vector.push_back(&_mission); - _navigation_mode_vector.push_back(&_loiter); - _navigation_mode_vector.push_back(&_rtl); + _navigation_mode_array[0] = &_mission; + _navigation_mode_array[1] = &_loiter; + _navigation_mode_array[2] = &_rtl; updateParams(); } @@ -369,11 +369,11 @@ Navigator::task_main() } /* iterate through navigation modes and set active/inactive for each */ - for(unsigned int i = 0; i < _navigation_mode_vector.size(); i++) { - if (_navigation_mode == _navigation_mode_vector[i]) { - _update_triplet = _navigation_mode_vector[i]->on_active(&_pos_sp_triplet); + for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { + if (_navigation_mode == _navigation_mode_array[i]) { + _update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet); } else { - _navigation_mode_vector[i]->on_inactive(); + _navigation_mode_array[i]->on_inactive(); } } -- cgit v1.2.3 From 51f05612ebf056a46d0838fed0dc562d3e5289ad Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Thu, 12 Jun 2014 16:33:15 -0700 Subject: Added some comments --- src/modules/navigator/navigator.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 929de0259..dadd15527 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -58,6 +58,10 @@ #include "rtl.h" #include "geofence.h" +/** + * Number of navigation modes that need on_active/on_inactive calls + * Currently: mission, loiter, and rtl + */ #define NAVIGATOR_MODE_ARRAY_SIZE 3 class Navigator : public control::SuperBlock -- cgit v1.2.3 From b6b3ad6e1e31503d230ad6e7bc8f2d7b5b596ad7 Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 13 Jun 2014 14:05:47 +0200 Subject: U-blox driver rework,, step 4 Config phase and parser rewrite --- src/drivers/gps/gps.cpp | 41 ++- src/drivers/gps/ubx.cpp | 903 ++++++++++++++++++++++++++---------------------- src/drivers/gps/ubx.h | 662 ++++++++++++++++++----------------- 3 files changed, 849 insertions(+), 757 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index f4d2f4b44..6ae325297 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -80,6 +80,14 @@ #endif static const int ERROR = -1; +/* class for dynamic allocation of satellite info data */ +class GPS_Sat_Info +{ +public: + struct satellite_info_s _data; +}; + + class GPS : public device::CDev { public: @@ -103,17 +111,17 @@ private: char _port[20]; ///< device / serial port path volatile int _task; //< worker task bool _healthy; ///< flag to signal if the GPS is ok - bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed bool _mode_changed; ///< flag that the GPS mode has changed gps_driver_mode_t _mode; ///< current mode GPS_Helper *_Helper; ///< instance of GPS parser - struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position + GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object + struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position - struct satellite_info_s _report_sat_info; ///< uORB topic for satellite info + struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate bool _fake_gps; ///< fake gps output - bool _enable_sat_info; ///< enable sat info /** @@ -165,11 +173,12 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : _mode_changed(false), _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), + _Sat_Info(nullptr), _report_gps_pos_pub(-1), + _p_report_sat_info(nullptr), _report_sat_info_pub(-1), _rate(0.0f), - _fake_gps(fake_gps), - _enable_sat_info(enable_sat_info) + _fake_gps(fake_gps) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -179,7 +188,13 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : /* we need this potentially before it could be set in task_main */ g_dev = this; memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); - memset(&_report_sat_info, 0, sizeof(_report_sat_info)); + + /* create satellite info data object if requested */ + if (enable_sat_info) { + _Sat_Info = new(GPS_Sat_Info); + _p_report_sat_info = &_Sat_Info->_data; + memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); + } _debug_enabled = true; } @@ -214,7 +229,7 @@ GPS::init() /* start the GPS driver worker task */ _task = task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr); + SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); @@ -320,7 +335,7 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, _enable_sat_info); + _Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: @@ -353,12 +368,12 @@ GPS::task_main() _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } - if (helper_ret & 2) { + if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub > 0) { - orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, &_report_sat_info); + orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); } else { - _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), &_report_sat_info); + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); } } } @@ -466,7 +481,7 @@ GPS::print_info() } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - warnx("sat info: %s", (_enable_sat_info) ? "enabled" : "disabled"); + warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 0708e23b4..1783ae0f7 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,14 +34,18 @@ /** * @file ubx.cpp * - * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description + * U-Blox protocol implementation. Following u-blox 6/7/8 Receiver Description * including Prototol Specification. * * @author Thomas Gubler * @author Julian Oes * @author Anton Babushkin * + * @author Hannes Delago + * (rework, add ubx7+ compatibility) + * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_%28UBX-13003221%29.pdf */ #include @@ -65,18 +69,28 @@ #define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls #define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval -#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) +#define SWAP16(X) ((((X) >> 8) & 0x00ff) | (((X) << 8) & 0xff00)) + +/**** Trace macros, disable for production builds */ +#define UBX_TRACE_PARSER(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* decoding progress in parse_char() */ +#define UBX_TRACE_RXMSG(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* Rx msgs in payload_rx_done() */ +#define UBX_TRACE_SVINFO(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* NAV-SVINFO processing (debug use only, will cause rx buffer overflows) */ + +/**** Warning macros, disable to save memory */ +#define UBX_WARN(s, ...) {warnx(s, ## __VA_ARGS__);} -UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info) : + +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) : _fd(fd), _gps_position(gps_position), _satellite_info(satellite_info), - _enable_sat_info(enable_sat_info), _configured(false), - _waiting_for_ack(false), + _ack_state(UBX_ACK_IDLE), _got_posllh(false), _got_velned(false), - _disable_cmd_last(0) + _disable_cmd_last(0), + _ack_waiting_msg(0) { decode_init(); } @@ -100,55 +114,41 @@ UBX::configure(unsigned &baudrate) /* flush input and wait for at least 20 ms silence */ decode_init(); - wait_for_ack(20); + receive(20); decode_init(); /* Send a CFG-PRT message to set the UBX protocol for in and out - * and leave the baudrate as it is, we just want an ACK-ACK from this - */ - type_gps_bin_cfg_prt_packet_t cfg_prt_packet; - /* Set everything else of the packet to 0, otherwise the module wont accept it */ - memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); - - _message_class_needed = UBX_CLASS_CFG; - _message_id_needed = UBX_MESSAGE_CFG_PRT; - - /* Define the package contents, don't change the baudrate */ - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = baudrate; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + * and leave the baudrate as it is, we just want an ACK-ACK for this */ + memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt)); + _buf.payload_tx_cfg_prt.portID = UBX_TX_CFG_PRT_PORTID; + _buf.payload_tx_cfg_prt.mode = UBX_TX_CFG_PRT_MODE; + _buf.payload_tx_cfg_prt.baudRate = baudrate; + _buf.payload_tx_cfg_prt.inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK; + _buf.payload_tx_cfg_prt.outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK; + + send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt)); + + if (wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false) < 0) { /* try next baudrate */ continue; } /* Send a CFG-PRT message again, this time change the baudrate */ + memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt)); + _buf.payload_tx_cfg_prt.portID = UBX_TX_CFG_PRT_PORTID; + _buf.payload_tx_cfg_prt.mode = UBX_TX_CFG_PRT_MODE; + _buf.payload_tx_cfg_prt.baudRate = UBX_TX_CFG_PRT_BAUDRATE; + _buf.payload_tx_cfg_prt.inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK; + _buf.payload_tx_cfg_prt.outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK; - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); + send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt)); /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ - wait_for_ack(UBX_CONFIG_TIMEOUT); + wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false); - if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { - set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); - baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + if (UBX_TX_CFG_PRT_BAUDRATE != baudrate) { + set_baudrate(_fd, UBX_TX_CFG_PRT_BAUDRATE); + baudrate = UBX_TX_CFG_PRT_BAUDRATE; } /* at this point we have correct baudrate on both ends */ @@ -156,94 +156,68 @@ UBX::configure(unsigned &baudrate) } if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) { - return 1; + return 1; // connection and/or baudrate detection failed } - /* send a CFG-RATE message to define update rate */ - type_gps_bin_cfg_rate_packet_t cfg_rate_packet; - memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); - - _message_class_needed = UBX_CLASS_CFG; - _message_id_needed = UBX_MESSAGE_CFG_RATE; + /* Send a CFG-RATE message to define update rate */ + memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate)); + _buf.payload_tx_cfg_rate.measRate = UBX_TX_CFG_RATE_MEASINTERVAL; + _buf.payload_tx_cfg_rate.navRate = UBX_TX_CFG_RATE_NAVRATE; + _buf.payload_tx_cfg_rate.timeRef = UBX_TX_CFG_RATE_TIMEREF; - cfg_rate_packet.clsID = UBX_CLASS_CFG; - cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; - cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; - cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL; - cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; - cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + send_message(UBX_MSG_CFG_RATE, _buf.raw, sizeof(_buf.payload_tx_cfg_rate)); - send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet)); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("CFG FAIL: RATE"); + if (wait_for_ack(UBX_MSG_CFG_RATE, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } /* send a NAV5 message to set the options for the internal filter */ - type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; - memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); - - _message_class_needed = UBX_CLASS_CFG; - _message_id_needed = UBX_MESSAGE_CFG_NAV5; - - cfg_nav5_packet.clsID = UBX_CLASS_CFG; - cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; - cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; - cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; - cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; - cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + memset(&_buf.payload_tx_cfg_nav5, 0, sizeof(_buf.payload_tx_cfg_nav5)); + _buf.payload_tx_cfg_nav5.mask = UBX_TX_CFG_NAV5_MASK; + _buf.payload_tx_cfg_nav5.dynModel = UBX_TX_CFG_NAV5_DYNMODEL; + _buf.payload_tx_cfg_nav5.fixMode = UBX_TX_CFG_NAV5_FIXMODE; - send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + send_message(UBX_MSG_CFG_NAV5, _buf.raw, sizeof(_buf.payload_tx_cfg_nav5)); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("CFG FAIL: NAV5"); + if (wait_for_ack(UBX_MSG_CFG_NAV5, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); + configure_message_rate(UBX_MSG_NAV_POSLLH, 1); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV POSLLH"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 5); + configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV TIMEUTC"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); + configure_message_rate(UBX_MSG_NAV_SOL, 1); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SOL"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); + configure_message_rate(UBX_MSG_NAV_VELNED, 1); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV VELNED"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } - if (_enable_sat_info) { - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); + configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SVINFO"); - return 1; - } + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; } - configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); + configure_message_rate(UBX_MSG_MON_HW, 1); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: MON HW"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } @@ -251,27 +225,36 @@ UBX::configure(unsigned &baudrate) return 0; } -int -UBX::wait_for_ack(unsigned timeout) +int // -1 = NAK, error or timeout, 0 = ACK +UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) { - _waiting_for_ack = true; - uint64_t time_end = hrt_absolute_time() + timeout * 1000; + int ret = -1; - while ((timeout = (time_end - hrt_absolute_time()) / 1000) > 0) { - if (receive(timeout) > 0) { - if (!_waiting_for_ack) { - return 1; - } + _ack_state = UBX_ACK_WAITING; + _ack_waiting_msg = msg; // memorize sent msg class&ID for ACK check + + hrt_abstime time_started = hrt_absolute_time(); + + while ((_ack_state == UBX_ACK_WAITING) && (hrt_absolute_time() < time_started + timeout * 1000)) { + receive(timeout); + } + + if (_ack_state == UBX_ACK_GOT_ACK) { + ret = 0; // ACK received ok + } else if (report) { + if (_ack_state == UBX_ACK_GOT_NAK) { + UBX_WARN("ubx msg 0x%04x NAK", SWAP16((unsigned)msg)); } else { - return -1; // timeout or error receiving, or NAK + UBX_WARN("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg)); } } - return -1; // timeout + _ack_state = UBX_ACK_IDLE; + return ret; } -int -UBX::receive(unsigned timeout) +int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled +UBX::receive(const unsigned timeout) { /* poll descriptor */ pollfd fds[1]; @@ -295,7 +278,7 @@ UBX::receive(unsigned timeout) if (ret < 0) { /* something went wrong when polling */ - warnx("poll error"); + UBX_WARN("ubx poll() err"); return -1; } else if (ret == 0) { @@ -323,433 +306,517 @@ UBX::receive(unsigned timeout) /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { - if (parse_char(buf[i]) > 0) { - handled |= handle_message(); - } + handled |= parse_char(buf[i]); } } } /* abort after timeout if no useful packets received */ if (time_started + timeout * 1000 < hrt_absolute_time()) { - warnx("timeout - no useful messages"); return -1; } } } -int -UBX::parse_char(uint8_t b) +int // 0 = decoding, 1 = message handled, 2 = sat info message handled +UBX::parse_char(const uint8_t b) { + int ret = 0; + switch (_decode_state) { - /* First, look for sync1 */ - case UBX_DECODE_UNINIT: - if (b == UBX_SYNC1) { - _decode_state = UBX_DECODE_GOT_SYNC1; - //### printf("\nA"); - } + /* Expecting Sync1 */ + case UBX_DECODE_SYNC1: + if (b == UBX_SYNC1) { // Sync1 found --> expecting Sync2 + UBX_TRACE_PARSER("\nA"); + _decode_state = UBX_DECODE_SYNC2; + } break; - /* Second, look for sync2 */ - case UBX_DECODE_GOT_SYNC1: - if (b == UBX_SYNC2) { - _decode_state = UBX_DECODE_GOT_SYNC2; - //### printf("B"); + /* Expecting Sync2 */ + case UBX_DECODE_SYNC2: + if (b == UBX_SYNC2) { // Sync2 found --> expecting Class + UBX_TRACE_PARSER("B"); + _decode_state = UBX_DECODE_CLASS; - } else { - /* Second start symbol was wrong, reset state machine */ + } else { // Sync1 not followed by Sync2: reset parser decode_init(); - /* don't return error, it can be just false sync1 */ - //### printf("-"); } + break; + /* Expecting Class */ + case UBX_DECODE_CLASS: + UBX_TRACE_PARSER("C"); + add_byte_to_checksum(b); // checksum is calculated for everything except Sync and Checksum bytes + _rx_msg = b; + _decode_state = UBX_DECODE_ID; break; - /* Now look for class */ - case UBX_DECODE_GOT_SYNC2: - /* everything except sync1 and sync2 needs to be added to the checksum */ + /* Expecting ID */ + case UBX_DECODE_ID: + UBX_TRACE_PARSER("D"); add_byte_to_checksum(b); - _message_class = b; - _decode_state = UBX_DECODE_GOT_CLASS; - //### printf("C"); + _rx_msg |= b << 8; + _decode_state = UBX_DECODE_LENGTH1; break; - case UBX_DECODE_GOT_CLASS: + /* Expecting first length byte */ + case UBX_DECODE_LENGTH1: + UBX_TRACE_PARSER("E"); add_byte_to_checksum(b); - _message_id = b; - _decode_state = UBX_DECODE_GOT_MESSAGEID; - //### printf("D"); + _rx_payload_length = b; + _decode_state = UBX_DECODE_LENGTH2; break; - case UBX_DECODE_GOT_MESSAGEID: + /* Expecting second length byte */ + case UBX_DECODE_LENGTH2: + UBX_TRACE_PARSER("F"); add_byte_to_checksum(b); - _payload_size = b; //this is the first length byte - _decode_state = UBX_DECODE_GOT_LENGTH1; - //### printf("E"); + _rx_payload_length |= b << 8; // calculate payload size + if (payload_rx_init() != 0) { // start payload reception + // payload will not be handled, discard message + decode_init(); + } else { + _decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1; + } break; - case UBX_DECODE_GOT_LENGTH1: + /* Expecting payload */ + case UBX_DECODE_PAYLOAD: + UBX_TRACE_PARSER("."); add_byte_to_checksum(b); - _payload_size += b << 8; // here comes the second byte of length - _decode_state = UBX_DECODE_GOT_LENGTH2; - //### printf("F"); + if (_rx_msg != UBX_MSG_NAV_SVINFO) { + ret = payload_rx_add(b); // add a payload byte + } else { + ret = payload_rx_add_svinfo(b); // add a svinfo payload byte + } + if (ret < 0) { + // payload not handled, discard message + decode_init(); + } else if (ret > 0) { + // payload complete, expecting checksum + _decode_state = UBX_DECODE_CHKSUM1; + } else { + // expecting more payload, stay in state UBX_DECODE_PAYLOAD + } + ret = 0; + break; + + /* Expecting first checksum byte */ + case UBX_DECODE_CHKSUM1: + if (_rx_ck_a != b) { + UBX_WARN("ubx checksum err"); + decode_init(); + } else { + _decode_state = UBX_DECODE_CHKSUM2; + } + break; + + /* Expecting second checksum byte */ + case UBX_DECODE_CHKSUM2: + if (_rx_ck_b != b) { + UBX_WARN("ubx checksum err"); + } else { + ret = payload_rx_done(); // finish payload processing + } + decode_init(); break; - case UBX_DECODE_GOT_LENGTH2: + default: + break; + } - /* Add to checksum if not yet at checksum byte */ - if (_rx_count < _payload_size) - add_byte_to_checksum(b); + return ret; +} - _rx_buffer[_rx_count] = b; +/** + * Start payload rx + */ +int // -1 = abort, 0 = continue +UBX::payload_rx_init() +{ + int ret = 0; - /* once the payload has arrived, we can process the information */ - if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes - /* compare checksum */ - if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { - //### printf("O\n"); - return 1; // message received successfully + _rx_state = UBX_RXMSG_HANDLE; // handle by default - } else { - warnx("checksum wrong"); - decode_init(); - return -1; - } + switch (_rx_msg) { + case UBX_MSG_NAV_POSLLH: + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + break; - } else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; - //### printf("."); + case UBX_MSG_NAV_SOL: + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + break; - } else { - warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); - decode_init(); - return -1; - } + case UBX_MSG_NAV_TIMEUTC: + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + break; + case UBX_MSG_NAV_SVINFO: + if (_satellite_info == nullptr) + _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else + memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info + break; + + case UBX_MSG_NAV_VELNED: + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + break; + + case UBX_MSG_MON_HW: + if ( (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */ + && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) /* u-blox 7+ msg format */ + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + break; + + case UBX_MSG_ACK_ACK: + if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + break; + + case UBX_MSG_ACK_NAK: + if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured break; default: + _rx_state = UBX_RXMSG_DISABLE; // disable all other messages break; } - return 0; // message decoding in progress + switch (_rx_state) { + case UBX_RXMSG_HANDLE: // handle message + case UBX_RXMSG_IGNORE: // ignore message but don't report error + ret = 0; + break; + + case UBX_RXMSG_DISABLE: // disable unexpected messages + UBX_WARN("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length); + + { + hrt_abstime t = hrt_absolute_time(); + + if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) { + /* don't attempt for every message to disable, some might not be disabled */ + _disable_cmd_last = t; + UBX_WARN("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg)); + configure_message_rate(_rx_msg, 0); + } + } + + ret = -1; // return error, abort handling this message + break; + + case UBX_RXMSG_ERROR_LENGTH: // error: invalid length + UBX_WARN("ubx msg 0x%04x invalid len %u", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length); + ret = -1; // return error, abort handling this message + break; + + default: // invalid message state + UBX_WARN("ubx internal err1"); + ret = -1; // return error, abort handling this message + break; + } + + return ret; } +/** + * Add payload rx byte + */ +int // -1 = error, 0 = ok, 1 = payload completed +UBX::payload_rx_add(const uint8_t b) +{ + int ret = 0; + + _buf.raw[_rx_payload_index] = b; + if (++_rx_payload_index >= _rx_payload_length) { + ret = 1; // payload received completely + } -int -UBX::handle_message() + return ret; +} + +/** + * Add svinfo payload rx byte + */ +int // -1 = error, 0 = ok, 1 = payload completed +UBX::payload_rx_add_svinfo(const uint8_t b) { int ret = 0; - if (_configured) { - /* handle only info messages when configured */ - switch (_message_class) { - case UBX_CLASS_NAV: - switch (_message_id) { - case UBX_MESSAGE_NAV_POSLLH: { - // printf("GOT NAV_POSLLH\n"); - gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - - _gps_position->lat = packet->lat; - _gps_position->lon = packet->lon; - _gps_position->alt = packet->height_msl; - _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m - _gps_position->timestamp_position = hrt_absolute_time(); - - _rate_count_lat_lon++; - - _got_posllh = true; - ret = 1; - break; - } + if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { + // Fill Part 1 buffer + _buf.raw[_rx_payload_index] = b; + } else { + if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { + // Part 1 complete: decode Part 1 buffer + _satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, SAT_INFO_MAX_SATELLITES); + UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh); + } + if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_svinfo_part2_t)) { + // Still room in _satellite_info: fill Part 2 buffer + unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(ubx_payload_rx_nav_svinfo_part2_t); + _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) { + // Part 2 complete: decode Part 2 buffer + unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(ubx_payload_rx_nav_svinfo_part2_t); + _satellite_info->used[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.flags & 0x01); + _satellite_info->snr[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.cno); + _satellite_info->elevation[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev); + _satellite_info->azimuth[sat_index] = (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f); + _satellite_info->svid[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid); + UBX_TRACE_SVINFO("SVINFO #%02u used %u snr %3u elevation %3u azimuth %3u svid %3u\n", + (unsigned)sat_index + 1, + (unsigned)_satellite_info->used[sat_index], + (unsigned)_satellite_info->snr[sat_index], + (unsigned)_satellite_info->elevation[sat_index], + (unsigned)_satellite_info->azimuth[sat_index], + (unsigned)_satellite_info->svid[sat_index] + ); + } + } + } - case UBX_MESSAGE_NAV_SOL: { - // printf("GOT NAV_SOL\n"); - gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + if (++_rx_payload_index >= _rx_payload_length) { + ret = 1; // payload received completely + } - _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; - _gps_position->timestamp_variance = hrt_absolute_time(); - _gps_position->satellites_used = packet->numSV; + return ret; +} - ret = 1; - break; - } +/** + * Finish payload rx + */ +int // 0 = no message handled, 1 = message handled, 2 = sat info message handled +UBX::payload_rx_done(void) +{ + int ret = 0; + + // return if no message handled + if (_rx_state != UBX_RXMSG_HANDLE) { + return ret; + } + + // handle message + switch (_rx_msg) { + + case UBX_MSG_NAV_POSLLH: + UBX_TRACE_RXMSG("Rx NAV-POSLLH\n"); + + _gps_position->lat = _buf.payload_rx_nav_posllh.lat; + _gps_position->lon = _buf.payload_rx_nav_posllh.lon; + _gps_position->alt = _buf.payload_rx_nav_posllh.hMSL; + _gps_position->eph_m = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m + _gps_position->epv_m = (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m + + _gps_position->timestamp_position = hrt_absolute_time(); + + _rate_count_lat_lon++; + _got_posllh = true; + + ret = 1; + break; + + case UBX_MSG_NAV_SOL: + UBX_TRACE_RXMSG("Rx NAV-SOL\n"); + + _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; + _gps_position->s_variance_m_s = _buf.payload_rx_nav_sol.sAcc; + _gps_position->p_variance_m = _buf.payload_rx_nav_sol.pAcc; + _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; + + _gps_position->timestamp_variance = hrt_absolute_time(); + + ret = 1; + break; - case UBX_MESSAGE_NAV_TIMEUTC: { - // printf("GOT NAV_TIMEUTC\n"); - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; + case UBX_MSG_NAV_TIMEUTC: + UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); - /* convert to unix timestamp */ - struct tm timeinfo; - timeinfo.tm_year = packet->year - 1900; - timeinfo.tm_mon = packet->month - 1; - timeinfo.tm_mday = packet->day; - timeinfo.tm_hour = packet->hour; - timeinfo.tm_min = packet->min; - timeinfo.tm_sec = packet->sec; - time_t epoch = mktime(&timeinfo); + { + /* convert to unix timestamp */ + struct tm timeinfo; + timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900; + timeinfo.tm_mon = _buf.payload_rx_nav_timeutc.month - 1; + timeinfo.tm_mday = _buf.payload_rx_nav_timeutc.day; + timeinfo.tm_hour = _buf.payload_rx_nav_timeutc.hour; + timeinfo.tm_min = _buf.payload_rx_nav_timeutc.min; + timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; + time_t epoch = mktime(&timeinfo); #ifndef CONFIG_RTC - //Since we lack a hardware RTC, set the system time clock based on GPS UTC - //TODO generalize this by moving into gps.cpp? - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = packet->time_nanoseconds; - clock_settime(CLOCK_REALTIME, &ts); + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; + clock_settime(CLOCK_REALTIME, &ts); #endif - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - _gps_position->timestamp_time = hrt_absolute_time(); + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f); + } - ret = 1; - break; - } + _gps_position->timestamp_time = hrt_absolute_time(); - case UBX_MESSAGE_NAV_SVINFO: { - //printf("GOT NAV_SVINFO\n"); - if (!_enable_sat_info) { // if satellite info processing not enabled: - break; // ignore and disable NAV-SVINFO message - } - const int length_part1 = 8; - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - - //printf("Number of Channels: %d\n", packet_part1->numCh); - for (_satellite_info->count = 0; - _satellite_info->count < MIN(packet_part1->numCh, sizeof(_satellite_info->snr) / sizeof(_satellite_info->snr[0])); - _satellite_info->count++) { - /* set pointer to satellite_i information */ - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + _satellite_info->count* length_part2]); - - /* write satellite information to global storage */ - _satellite_info->used[_satellite_info->count] = packet_part2->flags & 0x01; - _satellite_info->snr[_satellite_info->count] = packet_part2->cno; - _satellite_info->elevation[_satellite_info->count] = (uint8_t)(packet_part2->elev); - _satellite_info->azimuth[_satellite_info->count] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - _satellite_info->svid[_satellite_info->count] = packet_part2->svid; - //printf("SAT %d: %d %d %d %d\n", _satellite_info->count, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); - } - - _satellite_info->timestamp = hrt_absolute_time(); - - ret = 2; - break; - } + ret = 1; + break; - case UBX_MESSAGE_NAV_VELNED: { - // printf("GOT NAV_VELNED\n"); - gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + case UBX_MSG_NAV_SVINFO: + UBX_TRACE_RXMSG("Rx NAV-SVINFO\n"); - _gps_position->vel_m_s = (float)packet->speed * 1e-2f; - _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ - _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ - _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ - _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->vel_ned_valid = true; - _gps_position->timestamp_velocity = hrt_absolute_time(); + // _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp + _satellite_info->timestamp = hrt_absolute_time(); - _rate_count_vel++; + ret = 2; + break; - _got_velned = true; - ret = 1; - break; - } + case UBX_MSG_NAV_VELNED: + UBX_TRACE_RXMSG("Rx NAV-VELNED\n"); - default: - break; - } + _gps_position->vel_m_s = (float)_buf.payload_rx_nav_velned.speed * 1e-2f; + _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_velned.velN * 1e-2f; /* NED NORTH velocity */ + _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_velned.velE * 1e-2f; /* NED EAST velocity */ + _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_velned.velD * 1e-2f; /* NED DOWN velocity */ + _gps_position->cog_rad = (float)_buf.payload_rx_nav_velned.heading * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_velned.cAcc * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->vel_ned_valid = true; - break; + _gps_position->timestamp_velocity = hrt_absolute_time(); - case UBX_CLASS_ACK: { - /* ignore ACK when already configured */ - ret = 1; - break; - } + _rate_count_vel++; + _got_velned = true; - case UBX_CLASS_MON: - switch (_message_id) { + ret = 1; + break; - case UBX_MESSAGE_MON_HW: - switch (_payload_size) { + case UBX_MSG_MON_HW: + UBX_TRACE_RXMSG("Rx MON-HW\n"); - case UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE: /* u-blox 6 msg format */ - _gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->noisePerMS; - _gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->jamInd; - ret = 1; - break; + switch (_rx_payload_length) { - case UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE: /* u-blox 7+ msg format */ - _gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->noisePerMS; - _gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->jamInd; - ret = 1; - break; + case sizeof(ubx_payload_rx_mon_hw_ubx6_t): /* u-blox 6 msg format */ + _gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx6.noisePerMS; + _gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx6.jamInd; - default: // unexpected payload size: - ret = 1; // ignore but don't disable msg - break; - } - break; + ret = 1; + break; - default: - break; - } + case sizeof(ubx_payload_rx_mon_hw_ubx7_t): /* u-blox 7+ msg format */ + _gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx7.noisePerMS; + _gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx7.jamInd; + + ret = 1; break; - default: + default: // unexpected payload size: + ret = 0; // don't handle message break; } + break; - if (ret == 0) { - /* message not handled */ - warnx("ubx: message not handled: 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); - - hrt_abstime t = hrt_absolute_time(); + case UBX_MSG_ACK_ACK: + UBX_TRACE_RXMSG("Rx ACK-ACK\n"); - if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) { - /* don't attempt for every message to disable, some might not be disabled */ - _disable_cmd_last = t; - warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); - configure_message_rate(_message_class, _message_id, 0); - } + if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) { + _ack_state = UBX_ACK_GOT_ACK; } - } else { - /* handle only ACK while configuring */ - if (_message_class == UBX_CLASS_ACK) { - switch (_message_id) { - case UBX_MESSAGE_ACK_ACK: { - // printf("GOT ACK_ACK\n"); - gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; - - if (_waiting_for_ack) { - if (packet->clsID == _message_class_needed && packet->msgID == _message_id_needed) { - _waiting_for_ack = false; - ret = 1; - } - } - - break; - } + ret = 1; + break; - case UBX_MESSAGE_ACK_NAK: { - // printf("GOT ACK_NAK\n"); - warnx("ubx: not acknowledged"); - /* configuration obviously not successful */ - _waiting_for_ack = false; - break; - } + case UBX_MSG_ACK_NAK: + UBX_TRACE_RXMSG("Rx ACK-NAK\n"); - default: - break; - } + if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) { + _ack_state = UBX_ACK_GOT_NAK; } + + ret = 1; + break; + + default: + break; } - decode_init(); return ret; } void UBX::decode_init(void) { + _decode_state = UBX_DECODE_SYNC1; _rx_ck_a = 0; _rx_ck_b = 0; - _rx_count = 0; - _decode_state = UBX_DECODE_UNINIT; - _payload_size = 0; - /* don't reset _message_class, _message_id, _rx_buffer leave it for message handler */ + _rx_payload_length = 0; + _rx_payload_index = 0; } void -UBX::add_byte_to_checksum(uint8_t b) +UBX::add_byte_to_checksum(const uint8_t b) { _rx_ck_a = _rx_ck_a + b; _rx_ck_b = _rx_ck_b + _rx_ck_a; } void -UBX::add_checksum_to_message(uint8_t *message, const unsigned length) +UBX::calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum) { - uint8_t ck_a = 0; - uint8_t ck_b = 0; - unsigned i; - - for (i = 0; i < length - 2; i++) { - ck_a = ck_a + message[i]; - ck_b = ck_b + ck_a; + for (uint16_t i = 0; i < length; i++) { + checksum->ck_a = checksum->ck_a + buffer[i]; + checksum->ck_b = checksum->ck_b + checksum->ck_a; } - - /* the checksum is written to the last to bytes of a message */ - message[length - 2] = ck_a; - message[length - 1] = ck_b; } void -UBX::add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) +UBX::configure_message_rate(const uint16_t msg, const uint8_t rate) { - for (unsigned i = 0; i < length; i++) { - ck_a = ck_a + message[i]; - ck_b = ck_b + ck_a; - } -} + ubx_payload_tx_cfg_msg_t cfg_msg; // don't use _buf (allow interleaved operation) -void -UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) -{ - struct ubx_cfg_msg_rate msg; - msg.msg_class = msg_class; - msg.msg_id = msg_id; - msg.rate = rate; - send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); + cfg_msg.msg = msg; + cfg_msg.rate = rate; + + send_message(UBX_MSG_CFG_MSG, (uint8_t *)&cfg_msg, sizeof(cfg_msg)); } void -UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) +UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length) { - ssize_t ret = 0; + ubx_header_t header = {UBX_SYNC1, UBX_SYNC2}; + ubx_checksum_t checksum = {0, 0}; - /* calculate the checksum now */ - add_checksum_to_message(packet, length); + // Populate header + header.msg = msg; + header.length = length; - const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; + // Calculate checksum + calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes + calc_checksum(payload, length, &checksum); - /* start with the two sync bytes */ - ret += write(fd, sync_bytes, sizeof(sync_bytes)); - ret += write(fd, packet, length); - - if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? - warnx("ubx: configuration write fail"); -} - -void -UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) -{ - struct ubx_header header; - uint8_t ck_a = 0, ck_b = 0; - header.sync1 = UBX_SYNC1; - header.sync2 = UBX_SYNC2; - header.msg_class = msg_class; - header.msg_id = msg_id; - header.length = size; - - add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b); - add_checksum((uint8_t *)msg, size, ck_a, ck_b); - - /* configure ACK check */ - _message_class_needed = msg_class; - _message_id_needed = msg_id; - - write(_fd, (const char *)&header, sizeof(header)); - write(_fd, (const char *)msg, size); - write(_fd, (const char *)&ck_a, 1); - write(_fd, (const char *)&ck_b, 1); + // Send message + write(_fd, (const void *)&header, sizeof(header)); + write(_fd, (const void *)payload, length); + write(_fd, (const void *)&checksum, sizeof(checksum)); } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 50df3ada2..f084fc655 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,6 +41,9 @@ * @author Julian Oes * @author Anton Babushkin * + * @author Hannes Delago + * (rework, add ubx7+ compatibility) + * */ #ifndef UBX_H_ @@ -48,346 +51,341 @@ #include "gps_helper.h" -#define UBX_ENABLE_NAV_SVINFO 1 /* TODO: make this a command line option, allocate buffer(s) dynamically */ - #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 -/* ClassIDs (the ones that are used) */ -#define UBX_CLASS_NAV 0x01 -//#define UBX_CLASS_RXM 0x02 -#define UBX_CLASS_ACK 0x05 -#define UBX_CLASS_CFG 0x06 -#define UBX_CLASS_MON 0x0A - -/* MessageIDs (the ones that are used) */ -#define UBX_MESSAGE_NAV_POSLLH 0x02 -#define UBX_MESSAGE_NAV_SOL 0x06 -#define UBX_MESSAGE_NAV_VELNED 0x12 -#define UBX_MESSAGE_NAV_TIMEUTC 0x21 -#define UBX_MESSAGE_NAV_SVINFO 0x30 -#define UBX_MESSAGE_ACK_NAK 0x00 -#define UBX_MESSAGE_ACK_ACK 0x01 -#define UBX_MESSAGE_CFG_PRT 0x00 -#define UBX_MESSAGE_CFG_MSG 0x01 -#define UBX_MESSAGE_CFG_RATE 0x08 -#define UBX_MESSAGE_CFG_NAV5 0x24 -#define UBX_MESSAGE_MON_HW 0x09 - -/* Rx msg payload sizes */ -#define UBX_NAV_POSLLH_RX_PAYLOAD_SIZE 28 /**< NAV_POSLLH Rx msg payload size */ -#define UBX_NAV_SOL_RX_PAYLOAD_SIZE 52 /**< NAV_SOL Rx msg payload size */ -#define UBX_NAV_VELNED_RX_PAYLOAD_SIZE 36 /**< NAV_VELNED Rx msg payload size */ -#define UBX_NAV_TIMEUTC_RX_PAYLOAD_SIZE 20 /**< NAV_TIMEUTC Rx msg payload size */ -#define UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE 68 /**< MON_HW Rx msg payload size for u-blox 6 and below */ -#define UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE 60 /**< MON_HW Rx msg payload size for u-blox 7 and above */ -#define UBX_MAX_RX_PAYLOAD_SIZE 70 /**< arbitrary maximum for calculating parser buffer size w/o NAV_SVINFO active */ +/* Message Classes */ +#define UBX_CLASS_NAV 0x01 +#define UBX_CLASS_ACK 0x05 +#define UBX_CLASS_CFG 0x06 +#define UBX_CLASS_MON 0x0A + +/* Message IDs */ +#define UBX_ID_NAV_POSLLH 0x02 +#define UBX_ID_NAV_SOL 0x06 +#define UBX_ID_NAV_VELNED 0x12 +#define UBX_ID_NAV_TIMEUTC 0x21 +#define UBX_ID_NAV_SVINFO 0x30 +#define UBX_ID_ACK_NAK 0x00 +#define UBX_ID_ACK_ACK 0x01 +#define UBX_ID_CFG_PRT 0x00 +#define UBX_ID_CFG_MSG 0x01 +#define UBX_ID_CFG_RATE 0x08 +#define UBX_ID_CFG_NAV5 0x24 +#define UBX_ID_MON_HW 0x09 + +/* Message Classes & IDs */ +#define UBX_MSG_NAV_POSLLH ((UBX_CLASS_NAV) | UBX_ID_NAV_POSLLH << 8) +#define UBX_MSG_NAV_SOL ((UBX_CLASS_NAV) | UBX_ID_NAV_SOL << 8) +#define UBX_MSG_NAV_VELNED ((UBX_CLASS_NAV) | UBX_ID_NAV_VELNED << 8) +#define UBX_MSG_NAV_TIMEUTC ((UBX_CLASS_NAV) | UBX_ID_NAV_TIMEUTC << 8) +#define UBX_MSG_NAV_SVINFO ((UBX_CLASS_NAV) | UBX_ID_NAV_SVINFO << 8) +#define UBX_MSG_ACK_NAK ((UBX_CLASS_ACK) | UBX_ID_ACK_NAK << 8) +#define UBX_MSG_ACK_ACK ((UBX_CLASS_ACK) | UBX_ID_ACK_ACK << 8) +#define UBX_MSG_CFG_PRT ((UBX_CLASS_CFG) | UBX_ID_CFG_PRT << 8) +#define UBX_MSG_CFG_MSG ((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8) +#define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8) +#define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8) +#define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) /* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */ -#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */ -#define UBX_NAV_SVINFO_RX_PAYLOAD_SIZE (8 + 12 * UBX_MAX_NUM_SAT) /**< NAV_SVINFO Rx msg payload size */ - -/* CFG class Tx msg defs */ -#define UBX_CFG_PRT_LENGTH 20 -#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ -#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */ -#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ -#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ - -#define UBX_CFG_RATE_LENGTH 6 -#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200 /**< 200ms for 5Hz */ -#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */ -#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */ - -#define UBX_CFG_NAV5_LENGTH 36 -#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005 /**< XXX only update dynamic model and fix mode */ -#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ -#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ - -#define UBX_CFG_MSG_LENGTH 8 -#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ -#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ -#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10 - - -// ************ -/** the structures of the binary packets */ -#pragma pack(push, 1) +#define UBX_MAX_NUM_SAT_SVINFO 50 /**< Practical observed max number of satellites in SVNFO msg */ +#define UBX_PAYLOAD_NAV_SVINFO (8 + 12 * UBX_MAX_NUM_SAT_SVINFO) /**< NAV_SVINFO Rx msg payload size */ -struct ubx_header { - uint8_t sync1; - uint8_t sync2; - uint8_t msg_class; - uint8_t msg_id; - uint16_t length; -}; +/* TX CFG-PRT message contents */ +#define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ +#define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ +#define UBX_TX_CFG_PRT_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */ +#define UBX_TX_CFG_PRT_INPROTOMASK 0x01 /**< UBX in */ +#define UBX_TX_CFG_PRT_OUTPROTOMASK 0x01 /**< UBX out */ -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - int32_t lon; /**< Longitude * 1e-7, deg */ - int32_t lat; /**< Latitude * 1e-7, deg */ - int32_t height; /**< Height above Ellipsoid, mm */ - int32_t height_msl; /**< Height above mean sea level, mm */ - uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ - uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_posllh_packet_t; +/* TX CFG-RATE message contents */ +#define UBX_TX_CFG_RATE_MEASINTERVAL 200 /**< 200ms for 5Hz */ +#define UBX_TX_CFG_RATE_NAVRATE 1 /**< cannot be changed */ +#define UBX_TX_CFG_RATE_TIMEREF 0 /**< 0: UTC, 1: GPS time */ -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */ - int16_t week; /**< GPS week (GPS time) */ - uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ - uint8_t flags; - int32_t ecefX; - int32_t ecefY; - int32_t ecefZ; - uint32_t pAcc; - int32_t ecefVX; - int32_t ecefVY; - int32_t ecefVZ; - uint32_t sAcc; - uint16_t pDOP; - uint8_t reserved1; - uint8_t numSV; /**< Number of SVs used in Nav Solution */ - uint32_t reserved2; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_sol_packet_t; +/* TX CFG-NAV5 message contents */ +#define UBX_TX_CFG_NAV5_MASK 0x0005 /**< Only update dynamic model and fix mode */ +#define UBX_TX_CFG_NAV5_DYNMODEL 7 /**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */ +#define UBX_TX_CFG_NAV5_FIXMODE 2 /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */ -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */ - int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */ - uint16_t year; /**< Year, range 1999..2099 (UTC) */ - uint8_t month; /**< Month, range 1..12 (UTC) */ - uint8_t day; /**< Day of Month, range 1..31 (UTC) */ - uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */ - uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */ - uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */ - uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */ - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_timeutc_packet_t; - -//typedef struct { -// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ -// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ -// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ -// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ -// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ -// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ -// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ -// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ -// uint8_t ck_a; -// uint8_t ck_b; -//} gps_bin_nav_dop_packet_t; +/* TX CFG-MSG message contents */ +#define UBX_TX_CFG_MSG_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ +#define UBX_TX_CFG_MSG_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ +#define UBX_TX_CFG_MSG_RATE1_05HZ 10 -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint8_t numCh; /**< Number of channels */ - uint8_t globalFlags; - uint16_t reserved2; -} gps_bin_nav_svinfo_part1_packet_t; +/*** u-blox protocol binary message and payload definitions ***/ +#pragma pack(push, 1) +/* General: Header */ typedef struct { - uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ - uint8_t svid; /**< Satellite ID */ - uint8_t flags; - uint8_t quality; - uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */ - int8_t elev; /**< Elevation in integer degrees */ - int16_t azim; /**< Azimuth in integer degrees */ - int32_t prRes; /**< Pseudo range residual in centimetres */ - -} gps_bin_nav_svinfo_part2_packet_t; + uint8_t sync1; + uint8_t sync2; + uint16_t msg; + uint16_t length; +} ubx_header_t; +/* General: Checksum */ typedef struct { - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_svinfo_part3_packet_t; + uint8_t ck_a; + uint8_t ck_b; +} ubx_checksum_t ; +/* Rx NAV-POSLLH */ typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t velN; //NED north velocity, cm/s - int32_t velE; //NED east velocity, cm/s - int32_t velD; //NED down velocity, cm/s - uint32_t speed; //Speed (3-D), cm/s - uint32_t gSpeed; //Ground Speed (2-D), cm/s - int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5 - uint32_t sAcc; //Speed Accuracy Estimate, cm/s - uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5 - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_velned_packet_t; - -struct gps_bin_mon_hw_ubx6_packet { - uint32_t pinSel; - uint32_t pinBank; - uint32_t pinDir; - uint32_t pinVal; - uint16_t noisePerMS; - uint16_t agcCnt; - uint8_t aStatus; - uint8_t aPower; - uint8_t flags; - uint8_t __reserved1; - uint32_t usedMask; - uint8_t VP[25]; - uint8_t jamInd; - uint16_t __reserved3; - uint32_t pinIrq; - uint32_t pulLH; - uint32_t pullL; -}; - -struct gps_bin_mon_hw_ubx7_packet { - uint32_t pinSel; - uint32_t pinBank; - uint32_t pinDir; - uint32_t pinVal; - uint16_t noisePerMS; - uint16_t agcCnt; - uint8_t aStatus; - uint8_t aPower; - uint8_t flags; - uint8_t __reserved1; - uint32_t usedMask; - uint8_t VP[17]; - uint8_t jamInd; - uint16_t __reserved3; - uint32_t pinIrq; - uint32_t pulLH; - uint32_t pullL; -}; - -//typedef struct { -// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ -// int16_t week; /**< Measurement GPS week number */ -// uint8_t numVis; /**< Number of visible satellites */ -// -// //... rest of package is not used in this implementation -// -//} gps_bin_rxm_svsi_packet_t; - + uint32_t iTOW; /**< GPS Time of Week [ms] */ + int32_t lon; /**< Longitude [1e-7 deg] */ + int32_t lat; /**< Latitude [1e-7 deg] */ + int32_t height; /**< Height above ellipsoid [mm] */ + int32_t hMSL; /**< Height above mean sea level [mm] */ + uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */ + uint32_t vAcc; /**< Vertical accuracy estimate [mm] */ +} ubx_payload_rx_nav_posllh_t; + +/* Rx NAV-SOL */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_ack_ack_packet_t; - + uint32_t iTOW; /**< GPS Time of Week [ms] */ + int32_t fTOW; /**< Fractional part of iTOW (range: +/-500000) [ns] */ + int16_t week; /**< GPS week */ + uint8_t gpsFix; /**< GPSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ + uint8_t flags; + int32_t ecefX; + int32_t ecefY; + int32_t ecefZ; + uint32_t pAcc; + int32_t ecefVX; + int32_t ecefVY; + int32_t ecefVZ; + uint32_t sAcc; + uint16_t pDOP; + uint8_t reserved1; + uint8_t numSV; /**< Number of SVs used in Nav Solution */ + uint32_t reserved2; +} ubx_payload_rx_nav_sol_t; + +/* Rx NAV-TIMEUTC */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_ack_nak_packet_t; - + uint32_t iTOW; /**< GPS Time of Week [ms] */ + uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */ + int32_t nano; /**< Fraction of second, range -1e9 .. 1e9 (UTC) [ns] */ + uint16_t year; /**< Year, range 1999..2099 (UTC) */ + uint8_t month; /**< Month, range 1..12 (UTC) */ + uint8_t day; /**< Day of month, range 1..31 (UTC) */ + uint8_t hour; /**< Hour of day, range 0..23 (UTC) */ + uint8_t min; /**< Minute of hour, range 0..59 (UTC) */ + uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */ + uint8_t valid; /**< Validity Flags (see ubx documentation) */ +} ubx_payload_rx_nav_timeutc_t; + +/* Rx NAV-SVINFO Part 1 */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint8_t portID; - uint8_t res0; - uint16_t res1; - uint32_t mode; - uint32_t baudRate; - uint16_t inProtoMask; - uint16_t outProtoMask; - uint16_t flags; - uint16_t pad; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_prt_packet_t; + uint32_t iTOW; /**< GPS Time of Week [ms] */ + uint8_t numCh; /**< Number of channels */ + uint8_t globalFlags; + uint16_t reserved2; +} ubx_payload_rx_nav_svinfo_part1_t; +/* Rx NAV-SVINFO Part 2 (repeated) */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint16_t measRate; - uint16_t navRate; - uint16_t timeRef; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_rate_packet_t; - + uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ + uint8_t svid; /**< Satellite ID */ + uint8_t flags; + uint8_t quality; + uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength) [dbHz] */ + int8_t elev; /**< Elevation [deg] */ + int16_t azim; /**< Azimuth [deg] */ + int32_t prRes; /**< Pseudo range residual [cm] */ +} ubx_payload_rx_nav_svinfo_part2_t; + +/* Rx NAV-VELNED */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint16_t mask; - uint8_t dynModel; - uint8_t fixMode; - int32_t fixedAlt; - uint32_t fixedAltVar; - int8_t minElev; - uint8_t drLimit; - uint16_t pDop; - uint16_t tDop; - uint16_t pAcc; - uint16_t tAcc; - uint8_t staticHoldThresh; - uint8_t dgpsTimeOut; - uint32_t reserved2; - uint32_t reserved3; - uint32_t reserved4; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_nav5_packet_t; - + uint32_t iTOW; /**< GPS Time of Week [ms] */ + int32_t velN; /**< North velocity component [cm/s]*/ + int32_t velE; /**< East velocity component [cm/s]*/ + int32_t velD; /**< Down velocity component [cm/s]*/ + uint32_t speed; /**< Speed (3-D) [cm/s] */ + uint32_t gSpeed; /**< Ground speed (2-D) [cm/s] */ + int32_t heading; /**< Heading of motion 2-D [1e-5 deg] */ + uint32_t sAcc; /**< Speed accuracy estimate [cm/s] */ + uint32_t cAcc; /**< Course / Heading accuracy estimate [1e-5 deg] */ +} ubx_payload_rx_nav_velned_t; + +/* Rx MON-HW (ubx6) */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint8_t msgClass_payload; - uint8_t msgID_payload; - uint8_t rate; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_msg_packet_t; + uint32_t pinSel; + uint32_t pinBank; + uint32_t pinDir; + uint32_t pinVal; + uint16_t noisePerMS; + uint16_t agcCnt; + uint8_t aStatus; + uint8_t aPower; + uint8_t flags; + uint8_t reserved1; + uint32_t usedMask; + uint8_t VP[25]; + uint8_t jamInd; + uint16_t reserved3; + uint32_t pinIrq; + uint32_t pullH; + uint32_t pullL; +} ubx_payload_rx_mon_hw_ubx6_t; + +/* Rx MON-HW (ubx7+) */ +typedef struct { + uint32_t pinSel; + uint32_t pinBank; + uint32_t pinDir; + uint32_t pinVal; + uint16_t noisePerMS; + uint16_t agcCnt; + uint8_t aStatus; + uint8_t aPower; + uint8_t flags; + uint8_t reserved1; + uint32_t usedMask; + uint8_t VP[17]; + uint8_t jamInd; + uint16_t reserved3; + uint32_t pinIrq; + uint32_t pullH; + uint32_t pullL; +} ubx_payload_rx_mon_hw_ubx7_t; + +/* Rx ACK-ACK */ +typedef union { + uint16_t msg; + struct { + uint8_t clsID; + uint8_t msgID; + }; +} ubx_payload_rx_ack_ack_t; + +/* Rx ACK-NAK */ +typedef union { + uint16_t msg; + struct { + uint8_t clsID; + uint8_t msgID; + }; +} ubx_payload_rx_ack_nak_t; + +/* Tx CFG-PRT */ +typedef struct { + uint8_t portID; + uint8_t reserved0; + uint16_t txReady; + uint32_t mode; + uint32_t baudRate; + uint16_t inProtoMask; + uint16_t outProtoMask; + uint16_t flags; + uint16_t reserved5; +} ubx_payload_tx_cfg_prt_t; + +/* Tx CFG-RATE */ +typedef struct { + uint16_t measRate; /**< Measurement Rate, GPS measurements are taken every measRate milliseconds */ + uint16_t navRate; /**< Navigation Rate, in number of measurement cycles. This parameter cannot be changed, and must be set to 1 */ + uint16_t timeRef; /**< Alignment to reference time: 0 = UTC time, 1 = GPS time */ +} ubx_payload_tx_cfg_rate_t; -struct ubx_cfg_msg_rate { - uint8_t msg_class; - uint8_t msg_id; +/* Tx CFG-NAV5 */ +typedef struct { + uint16_t mask; + uint8_t dynModel; /**< Dynamic Platform model: 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */ + uint8_t fixMode; /**< Position Fixing Mode: 1 2D only, 2 3D only, 3 Auto 2D/3D */ + int32_t fixedAlt; + uint32_t fixedAltVar; + int8_t minElev; + uint8_t drLimit; + uint16_t pDop; + uint16_t tDop; + uint16_t pAcc; + uint16_t tAcc; + uint8_t staticHoldThresh; + uint8_t dgpsTimeOut; + uint8_t cnoThreshNumSVs; /**< (ubx7+ only, else 0) */ + uint8_t cnoThresh; /**< (ubx7+ only, else 0) */ + uint16_t reserved; + uint16_t staticHoldMaxDist; /**< (ubx8+ only, else 0) */ + uint8_t utcStandard; /**< (ubx8+ only, else 0) */ + uint8_t reserved3; + uint32_t reserved4; +} ubx_payload_tx_cfg_nav5_t; + +/* Tx CFG-MSG */ +typedef struct { + union { + uint16_t msg; + struct { + uint8_t msgClass; + uint8_t msgID; + }; + }; uint8_t rate; -}; +} ubx_payload_tx_cfg_msg_t; + +/* General message and payload buffer union */ +typedef union { + ubx_payload_rx_nav_posllh_t payload_rx_nav_posllh; + ubx_payload_rx_nav_sol_t payload_rx_nav_sol; + ubx_payload_rx_nav_timeutc_t payload_rx_nav_timeutc; + ubx_payload_rx_nav_svinfo_part1_t payload_rx_nav_svinfo_part1; + ubx_payload_rx_nav_svinfo_part2_t payload_rx_nav_svinfo_part2; + ubx_payload_rx_nav_velned_t payload_rx_nav_velned; + ubx_payload_rx_mon_hw_ubx6_t payload_rx_mon_hw_ubx6; + ubx_payload_rx_mon_hw_ubx7_t payload_rx_mon_hw_ubx7; + ubx_payload_rx_ack_ack_t payload_rx_ack_ack; + ubx_payload_rx_ack_nak_t payload_rx_ack_nak; + ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt; + ubx_payload_tx_cfg_rate_t payload_tx_cfg_rate; + ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5; + ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg; + uint8_t raw[]; +} ubx_buf_t; +#pragma pack(pop) +/*** END OF u-blox protocol binary message and payload definitions ***/ -// END the structures of the binary packets -// ************ - +/* Decoder state */ typedef enum { - UBX_DECODE_UNINIT = 0, - UBX_DECODE_GOT_SYNC1, - UBX_DECODE_GOT_SYNC2, - UBX_DECODE_GOT_CLASS, - UBX_DECODE_GOT_MESSAGEID, - UBX_DECODE_GOT_LENGTH1, - UBX_DECODE_GOT_LENGTH2 + UBX_DECODE_SYNC1 = 0, + UBX_DECODE_SYNC2, + UBX_DECODE_CLASS, + UBX_DECODE_ID, + UBX_DECODE_LENGTH1, + UBX_DECODE_LENGTH2, + UBX_DECODE_PAYLOAD, + UBX_DECODE_CHKSUM1, + UBX_DECODE_CHKSUM2 } ubx_decode_state_t; -//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; -#pragma pack(pop) +/* Rx message state */ +typedef enum { + UBX_RXMSG_IGNORE = 0, + UBX_RXMSG_HANDLE, + UBX_RXMSG_DISABLE, + UBX_RXMSG_ERROR_LENGTH +} ubx_rxmsg_state_t; + + +/* ACK state */ +typedef enum { + UBX_ACK_IDLE = 0, + UBX_ACK_WAITING, + UBX_ACK_GOT_ACK, + UBX_ACK_GOT_NAK +} ubx_ack_state_t; -/* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */ -/* TODO: make this a command line option, allocate buffer dynamically */ -#define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */ -#if (UBX_ENABLE_NAV_SVINFO != 0) - #define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) -#else - #define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) -#endif class UBX : public GPS_Helper { public: - UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info); + UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info); ~UBX(); - int receive(unsigned timeout); + int receive(const unsigned timeout); int configure(unsigned &baudrate); private: @@ -395,12 +393,23 @@ private: /** * Parse the binary UBX packet */ - int parse_char(uint8_t b); + int parse_char(const uint8_t b); + + /** + * Start payload rx + */ + int payload_rx_init(void); + + /** + * Add payload rx byte + */ + int payload_rx_add(const uint8_t b); + int payload_rx_add_svinfo(const uint8_t b); /** - * Handle the package once it has arrived + * Finish payload rx */ - int handle_message(void); + int payload_rx_done(void); /** * Reset the parse state machine for a fresh start @@ -410,45 +419,46 @@ private: /** * While parsing add every byte (except the sync bytes) to the checksum */ - void add_byte_to_checksum(uint8_t); + void add_byte_to_checksum(const uint8_t); /** - * Add the two checksum bytes to an outgoing message + * Send a message */ - void add_checksum_to_message(uint8_t *message, const unsigned length); + void send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length); /** - * Helper to send a config packet + * Configure message rate */ - void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); - - void configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate); + void configure_message_rate(const uint16_t msg, const uint8_t rate); - void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); - - void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); + /** + * Calculate & add checksum for given buffer + */ + void calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum); - int wait_for_ack(unsigned timeout); + /** + * Wait for message acknowledge + */ + int wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report); int _fd; struct vehicle_gps_position_s *_gps_position; struct satellite_info_s *_satellite_info; bool _enable_sat_info; bool _configured; - bool _waiting_for_ack; + ubx_ack_state_t _ack_state; bool _got_posllh; bool _got_velned; - uint8_t _message_class_needed; - uint8_t _message_id_needed; ubx_decode_state_t _decode_state; - uint8_t _rx_buffer[RECV_BUFFER_SIZE]; - unsigned _rx_count; + uint16_t _rx_msg; + ubx_rxmsg_state_t _rx_state; + uint16_t _rx_payload_length; + uint16_t _rx_payload_index; uint8_t _rx_ck_a; uint8_t _rx_ck_b; - uint8_t _message_class; - uint8_t _message_id; - unsigned _payload_size; hrt_abstime _disable_cmd_last; + uint16_t _ack_waiting_msg; + ubx_buf_t _buf; }; #endif /* UBX_H_ */ -- cgit v1.2.3 From e14d4751423e21a9ce052e1f6760f929d3694515 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 13 Jun 2014 14:31:19 +0200 Subject: navigator: update Loiter navigator mode to use new method names --- src/modules/navigator/loiter.cpp | 6 +++--- src/modules/navigator/loiter.h | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 035d035e1..8bbb79d5e 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -59,7 +59,7 @@ Loiter::Loiter(Navigator *navigator, const char *name) : /* load initial params */ updateParams(); /* initial reset */ - reset(); + on_inactive(); } Loiter::~Loiter() @@ -67,14 +67,14 @@ Loiter::~Loiter() } bool -Loiter::update(struct position_setpoint_triplet_s *pos_sp_triplet) +Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { /* set loiter item, don't reuse an existing position setpoint */ return set_loiter_item(false, pos_sp_triplet);; } void -Loiter::reset() +Loiter::on_inactive() { } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index a83b53f43..685ae6141 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -63,12 +63,12 @@ public: /** * This function is called while the mode is inactive */ - bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_inactive(); /** * This function is called while the mode is active */ - void reset(); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); }; #endif -- cgit v1.2.3 From 7724e3d889eecd6733c6e85341a9f0b97faa37cf Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 13 Jun 2014 23:18:58 +0200 Subject: Remove unreferenced SVINFO length defines --- src/drivers/gps/ubx.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index f084fc655..8fd8d4da6 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -88,10 +88,6 @@ #define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8) #define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) -/* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */ -#define UBX_MAX_NUM_SAT_SVINFO 50 /**< Practical observed max number of satellites in SVNFO msg */ -#define UBX_PAYLOAD_NAV_SVINFO (8 + 12 * UBX_MAX_NUM_SAT_SVINFO) /**< NAV_SVINFO Rx msg payload size */ - /* TX CFG-PRT message contents */ #define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ #define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -- cgit v1.2.3 From 195c4005e1bae41fcead6ff609f05a1058e6256d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 13 Jun 2014 23:36:26 +0200 Subject: mtecs: update default params --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index c95bf1dc9..bbbb8f9db 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -65,7 +65,7 @@ PARAM_DEFINE_INT32(MT_ENABLED, 1); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f); +PARAM_DEFINE_FLOAT(MT_THR_FF, 0.7f); /** * Total Energy Rate Control P @@ -75,7 +75,7 @@ PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f); +PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f); /** * Total Energy Rate Control I @@ -85,7 +85,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f); +PARAM_DEFINE_FLOAT(MT_THR_I, 0.25f); /** * Total Energy Rate Control Offset (Cruise throttle sp) @@ -104,7 +104,7 @@ PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.1f); +PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.4f); /** * Energy Distribution Rate Control P @@ -182,7 +182,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); * @max 10.0f * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f); +PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f); /** * D gain for the altitude control @@ -210,7 +210,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f); * @unit deg * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_FPA_MIN, -10.0f); +PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f); /** * Maximal flight path angle setpoint @@ -237,7 +237,7 @@ PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f); * @max 10.0f * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_ACC_P, 1.5f); +PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f); /** * D gain for the airspeed control -- cgit v1.2.3 From 91b590ef584cfc67be7555e3d7272bb94bc9b2b4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 13 Jun 2014 23:40:48 +0200 Subject: Move MISSION_STATE read/write from mavlink to navigator and commander --- src/modules/commander/commander.cpp | 22 ++++-- src/modules/dataman/dataman.h | 12 +--- src/modules/mavlink/mavlink_main.cpp | 119 ++++++------------------------- src/modules/navigator/mission.cpp | 33 +++++++++ src/modules/navigator/mission.h | 5 ++ src/modules/navigator/navigator_main.cpp | 7 -- 6 files changed, 80 insertions(+), 118 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e992706ac..d2f8c2994 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -89,6 +90,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -691,6 +693,11 @@ int commander_thread_main(int argc, char *argv[]) /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + if (status_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } /* armed topic */ orb_advert_t armed_pub; @@ -708,10 +715,17 @@ int commander_thread_main(int argc, char *argv[]) struct home_position_s home; memset(&home, 0, sizeof(home)); - if (status_pub < 0) { - warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); - warnx("exiting."); - exit(ERROR); + /* init mission state */ + mission_s mission; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { + if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { + mavlink_log_info(mavlink_fd, "[cmd] dataman ID: %i, count: %u, current: %i", + mission.dataman_id, mission.count, mission.current_index); + orb_advert_t mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + close(mission_pub); + } else { + mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + } } mavlink_log_info(mavlink_fd, "[cmd] started"); diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 215ec4c76..dbace21ef 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -53,7 +53,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ - DM_KEY_MISSION_STATE, /* Persistent mission state */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -127,16 +127,6 @@ extern "C" { dm_reset_reason restart_type /* The last reset type */ ); - /* NOTE: The following structure defines the persistent system state data stored in the single - entry DM_KEY_SYSTEM_STATE_KEY item type. It contains global system state information that - needs to survive restarts. This definition is application specific so it doesn't really belong - in this header, but till I find it a better home here it is */ - - typedef struct { - char offboard_waypoint_id; /* the index of the active offboard waypoint data */ - /* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */ - } persistent_system_state_t; - #ifdef __cplusplus } #endif diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5c6ef68ee..bc4376b84 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -18,7 +18,6 @@ * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, @@ -964,8 +963,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { - persistent_system_state_t sys_state; - /* init WPM state */ state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; @@ -980,40 +977,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - int sys_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)); - if (sys_state_size == sizeof(sys_state)) { - if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) { - state->dataman_id = sys_state.offboard_waypoint_id; - - if (_verbose) { warnx("WPM init: using dataman ID %d", state->dataman_id); } - - /* count waypoints in current waypoints storage */ - dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - struct mission_item_s mission_item; - - int seq = 0; - while (seq < state->max_size && dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { - seq++; - } - - if (_verbose) { warnx("WPM init: found %d items", seq); } - - state->size = seq; - - } else { - if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in MISSION_STATE", sys_state.offboard_waypoint_id); } - } - - } else { - if (_verbose) { warnx("WPM init: dataman MISSION_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } - } - - /* init mission topic */ - mission.count = state->size; - mission.dataman_id = state->dataman_id; - mission.current_index = -1; // TODO store current index in dataman? - - publish_mission(); + } /* @@ -1081,10 +1045,10 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) { - dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; struct mission_item_s mission_item; - if (dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { /* create mission_item_s from mavlink_mission_item_t */ mavlink_mission_item_t wp; @@ -1103,7 +1067,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD"); - if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, dm_id); } + if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, _wpm->dataman_id); } } } @@ -1214,9 +1178,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mission.current_index = wpc.seq; publish_mission(); - /* don't answer yet, wait for the navigator to respond, then publish the mission_result */ -// mavlink_wpm_send_waypoint_current(wpc.seq); - } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); } @@ -1392,6 +1353,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_count = wpc.count; _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + mission.current_index = -1; + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } @@ -1442,10 +1405,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - dm_item_t dm_id = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - if (dm_write(dm_id, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, dm_id); } + if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _wpm->current_dataman_id); } mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); @@ -1453,6 +1416,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } + /* waypoint marked as current */ + if (wp.current) { + mission.current_index = wp.seq; + } + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } _wpm->current_wp_id = wp.seq + 1; @@ -1463,65 +1431,23 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - /* write terminator item */ - if (_wpm->current_count < _wpm->max_size) { - if (dm_write(dm_id, _wpm->current_count, DM_PERSIST_POWER_ON_RESET, nullptr, 0) != 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR writing terminator item"); } + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - break; - } - } + /* update WPM state */ + _wpm->dataman_id = _wpm->current_dataman_id; + _wpm->size = _wpm->current_count; - /* offboard mission data saved correctly, now update the persistent system state */ - persistent_system_state_t state; - bool dm_result; + /* update mission topic */ + mission.dataman_id = _wpm->dataman_id; + mission.count = _wpm->current_count; - /* since we are doing a read-modify-write we must lock the item type */ - dm_lock(DM_KEY_MISSION_STATE); - - /* first read in the current state data, there may eventually be data other than the offboard index - * and we must preserve it */ - ssize_t dm_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &state, sizeof(state)); - if (dm_state_size != sizeof(state)) { - warnx("dataman MISSION_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); - } - - /* set new dataman storage ID */ - state.offboard_waypoint_id = _wpm->current_dataman_id; - - /* write back to dataman */ - dm_result = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); - - dm_unlock(DM_KEY_MISSION_STATE); - - if (dm_result) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman MISSION_STATE", state.offboard_waypoint_id); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); - - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - /* update WPM state */ - _wpm->dataman_id = _wpm->current_dataman_id; - _wpm->size = _wpm->current_count; - - /* update mission topic */ - mission.dataman_id = _wpm->dataman_id; - mission.count = _wpm->current_count; - mission.current_index = 0; - - publish_mission(); - } + publish_mission(); } else { /* request next item */ mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } - break; } @@ -1545,6 +1471,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->size = 0; /* update mission topic */ + mission.dataman_id = _wpm->dataman_id; mission.count = 0; mission.current_index = -1; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9244063b1..d9d8353f6 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -430,6 +430,37 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio return false; } +void +Mission::save_offboard_mission_state() +{ + mission_s mission_state; + + /* lock MISSION_STATE item */ + dm_lock(DM_KEY_MISSION_STATE); + + /* read current state */ + int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + + /* check if state actually changed to save flash write cycles */ + if (read_res != sizeof(mission_s) || mission_state.dataman_id != _offboard_mission.dataman_id || + mission_state.count != _offboard_mission.count || + mission_state.current_index != _current_offboard_mission_index) { + + mission_state.dataman_id = _offboard_mission.dataman_id; + mission_state.count = _offboard_mission.count; + mission_state.current_index = _current_offboard_mission_index; + + /* write modifyed state only if changed */ + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + mavlink_log_critical(_navigator->get_mavlink_fd(), "error saving mission state"); + + } + } + + /* unlock MISSION_STATE item */ + dm_unlock(DM_KEY_MISSION_STATE); +} + void Mission::report_mission_item_reached() { @@ -445,6 +476,8 @@ Mission::report_current_offboard_mission_item() { _mission_result.index_current_mission = _current_offboard_mission_index; publish_mission_result(); + + save_offboard_mission_state(); } void diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 322aaf96a..98b11bade 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -141,6 +141,11 @@ private: bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item); + /** + * Save current offboard mission state to dataman + */ + void save_offboard_mission_state(); + /** * Report that a mission item has been reached */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 888c8e7f4..a3c190c7f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -227,13 +227,6 @@ Navigator::task_main() warnx("Could not clear geofence"); } - /* Get the last offboard mission id */ - persistent_system_state_t sys_state; - if (dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { - if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) - _mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id); - } - /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); -- cgit v1.2.3 From ffd9ac7e081aebb3d6329a0f6c09812d1c0c4523 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 14 Jun 2014 01:31:23 +0200 Subject: mavlink: fix WPM initialization --- src/modules/mavlink/mavlink_main.cpp | 59 ++++++++++++++++++++---------------- src/modules/mavlink/mavlink_main.h | 4 ++- 2 files changed, 36 insertions(+), 27 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bc4376b84..126c4dfc6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -72,8 +72,6 @@ #include #include -#include -#include #include "mavlink_bridge_header.h" #include "mavlink_main.h" @@ -245,7 +243,7 @@ Mavlink::Mavlink() : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { _wpm = &_wpm_s; - mission.count = 0; + fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; _instance_id = Mavlink::instance_count(); @@ -860,10 +858,10 @@ void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + _mission_pub = orb_advertise(ORB_ID(offboard_mission), &_mission); } else { - orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission); + orb_publish(ORB_ID(offboard_mission), _mission_pub, &_mission); } } @@ -963,10 +961,20 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { + /* get offboard_mission topic */ + int mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + if (orb_copy(ORB_ID(offboard_mission), mission_sub, &_mission)) { + /* error getting topic, init to safe values */ + _mission.dataman_id = 0; + _mission.count = 0; + _mission.current_index = -1; + } + close(mission_sub); + /* init WPM state */ - state->size = 0; + state->size = _mission.count; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->dataman_id = 0; + state->dataman_id = _mission.dataman_id; state->current_state = MAVLINK_WPM_STATE_IDLE; state->current_partner_sysid = 0; state->current_partner_compid = 0; @@ -977,7 +985,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - + warnx("wpm init: dataman_id=%d, count=%u, current=%d", state->dataman_id, state->size, _mission.current_index); } /* @@ -1035,7 +1043,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin wpc.target_system = sysid; wpc.target_component = compid; - wpc.count = mission.count; + wpc.count = _wpm->size; mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); mavlink_missionlib_send_message(&msg); @@ -1058,6 +1066,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t wp.target_system = sysid; wp.target_component = compid; wp.seq = seq; + wp.current = (_mission_result.index_current_mission == seq) ? 1 : 0; mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); mavlink_missionlib_send_message(&msg); @@ -1175,7 +1184,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpc.seq < _wpm->size) { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT OK (%d)", wpc.seq); } - mission.current_index = wpc.seq; + _mission.current_index = wpc.seq; publish_mission(); } else { @@ -1353,7 +1362,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_count = wpc.count; _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission - mission.current_index = -1; + _mission.current_index = -1; mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } @@ -1418,7 +1427,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) /* waypoint marked as current */ if (wp.current) { - mission.current_index = wp.seq; + _mission.current_index = wp.seq; } if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } @@ -1438,8 +1447,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->size = _wpm->current_count; /* update mission topic */ - mission.dataman_id = _wpm->dataman_id; - mission.count = _wpm->current_count; + _mission.dataman_id = _wpm->dataman_id; + _mission.count = _wpm->current_count; publish_mission(); @@ -1471,9 +1480,9 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->size = 0; /* update mission topic */ - mission.dataman_id = _wpm->dataman_id; - mission.count = 0; - mission.current_index = -1; + _mission.dataman_id = _wpm->dataman_id; + _mission.count = 0; + _mission.current_index = -1; publish_mission(); @@ -1754,7 +1763,6 @@ Mavlink::pass_message(mavlink_message_t *msg) } - int Mavlink::task_main(int argc, char *argv[]) { @@ -1918,8 +1926,7 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_wpm_init(_wpm); int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - struct mission_result_s mission_result; - memset(&mission_result, 0, sizeof(mission_result)); + memset(&_mission_result, 0, sizeof(_mission_result)); _task_running = true; @@ -2027,19 +2034,19 @@ Mavlink::task_main(int argc, char *argv[]) orb_check(mission_result_sub, &updated); if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + orb_copy(ORB_ID(mission_result), mission_result_sub, &_mission_result); - if (_verbose) { warnx("Got mission result: new current: %d", mission_result.index_current_mission); } + if (_verbose) { warnx("WPM: got mission result, new current: %d", _mission_result.index_current_mission); } - if (mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); + if (_mission_result.mission_reached) { + mavlink_wpm_send_waypoint_reached((uint16_t)_mission_result.mission_index_reached); } - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); } else { if (slow_rate_limiter.check(t)) { - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 61fd7afe2..5d7273abd 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -50,6 +50,7 @@ #include #include +#include #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" @@ -238,7 +239,8 @@ private: MavlinkStream *_streams; orb_advert_t _mission_pub; - struct mission_s mission; + struct mission_s _mission; + struct mission_result_s _mission_result; MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; -- cgit v1.2.3 From 7d05f2df7cf50dea6d2960001b5b0af7236f9e5e Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Fri, 13 Jun 2014 20:38:43 -0400 Subject: Added support for velocity setpoint in mavlink_receiver and mc_pos_control --- src/modules/mavlink/mavlink_receiver.cpp | 36 +++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 2 ++ src/modules/mc_pos_control/mc_pos_control_main.cpp | 28 ++++++++++++++++- 3 files changed, 64 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index de9a6e7ad..8a00509a4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -103,6 +103,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _flow_pub(-1), _offboard_control_sp_pub(-1), _local_pos_sp_pub(-1), + _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), _vicon_position_pub(-1), @@ -450,6 +451,11 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message _rates_sp_pub = -1; } + if (_global_vel_sp_pub > 0) { + close(_global_vel_sp_pub); + _global_vel_sp_pub = -1; + } + if (_local_pos_sp_pub < 0) { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub); @@ -458,7 +464,30 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } } else if (_control_mode.flag_control_velocity_enabled) { - // TODO + /* velocity control */ + struct vehicle_global_velocity_setpoint_s global_vel_sp; + memset(&global_vel_sp, 0, sizeof(&global_vel_sp)); + + global_vel_sp.vx = offboard_control_sp.p1; + global_vel_sp.vy = offboard_control_sp.p2; + global_vel_sp.vz = offboard_control_sp.p3; + + if (_att_sp_pub > 0) { + close(_att_sp_pub); + _att_sp_pub = -1; + } + + if (_rates_sp_pub > 0) { + close(_rates_sp_pub); + _rates_sp_pub = -1; + } + + if (_global_vel_sp_pub < 0) { + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint_s), &_global_vel_sp_pub); + + } else { + orb_publish(ORB_ID(vehicle_global_velocity_setpoint_s), _global_vel_sp_pub, &global_vel_sp); + } } else if (_control_mode.flag_control_attitude_enabled) { /* attitude control */ @@ -1038,6 +1067,11 @@ MavlinkReceiver::receive_thread(void *arg) _local_pos_sp_pub = -1; } + if (_global_vel_sp_pub > 0) { + close(_global_vel_sp_pub); + _global_vel_sp_pub = -1; + } + if (_att_sp_pub > 0) { close(_att_sp_pub); _att_sp_pub = -1; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index db9e06461..a20cbc7e9 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -138,6 +139,7 @@ private: orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; orb_advert_t _local_pos_sp_pub; + orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; orb_advert_t _vicon_position_pub; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 9d9d2a7c0..53394b41c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -119,6 +119,7 @@ private: int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ int _local_pos_sp_sub; /**< offboard local position setpoint */ + int _global_vel_sp_sub; /**< offboard global velocity setpoint */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ @@ -257,6 +258,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _manual_sub(-1), _arming_sub(-1), _local_pos_sub(-1), + _global_vel_sp_sub(-1), _pos_sp_triplet_sub(-1), /* publications */ @@ -784,6 +786,24 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _params.land_speed; } + /* Offboard velocity control mode */ + if (_control_mode.flag_control_offboard_enabled) { + bool updated; + orb_check(_global_vel_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_sub, &_global_vel_sp); + } + + if (!_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_climb_rate_enabled) { + _vel_sp(2) = _global_vel_sp.vz; + } + + if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) { + _vel_sp(0) = _global_vel_sp.vx; + _vel_sp(1) = _global_vel_sp.vy; + } + if (!_control_mode.flag_control_manual_enabled) { /* limit 3D speed only in non-manual modes */ float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); @@ -801,10 +821,16 @@ MulticopterPositionControl::task_main() if (_global_vel_sp_pub > 0) { orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); - } else { + } else if (!_control_mode.flag_control_offboard_enabled) { _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); } + /* Close fd to let offboard vel sp be advertised in mavlink receiver */ + if (_control_mode.flag_control_offboard_enabled && _global_vel_sp_pub > 0) { + close (_global_vel_sp); + _global_vel_sp_pub = -1; + } + if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { /* reset integrals if needed */ if (_control_mode.flag_control_climb_rate_enabled) { -- cgit v1.2.3 From 6d8dfd78f11fdff038f7f7504c507d25429db195 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Jun 2014 15:54:24 +0200 Subject: sdlog2/mtecs: fix length of field name --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index c42ff0afe..5645ebcf1 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -431,7 +431,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), + LOG_FORMAT(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ -- cgit v1.2.3 From 6cf890b46b5a112ed20b3f8ed7be7b658d5c4a8c Mon Sep 17 00:00:00 2001 From: andre-nguyen Date: Sat, 14 Jun 2014 15:27:07 -0400 Subject: indentation and fix commander flags. It's impossible to control position at the same time as attitude so we have to disable some things. My logic is that all the control flags for position control should be opposite of the attitude control mode. --- src/modules/commander/commander.cpp | 4 ++-- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3da43a792..2c36b2e6c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1834,8 +1834,8 @@ set_control_mode() control_mode.flag_control_velocity_enabled = true; break; case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = true; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 9d9d2a7c0..6721d017d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -679,12 +679,12 @@ MulticopterPositionControl::task_main() /* Make sure position control is selected i.e. not only velocity control */ if (_control_mode.flag_control_position_enabled) { - _pos_sp(0) = _local_pos_sp.x; - _pos_sp(1) = _local_pos_sp.y; + _pos_sp(0) = _local_pos_sp.x; + _pos_sp(1) = _local_pos_sp.y; } if (_control_mode.flag_control_altitude_enabled) { - _pos_sp(2) = _local_pos_sp.z; + _pos_sp(2) = _local_pos_sp.z; } _att_sp.yaw_body = _local_pos_sp.yaw; -- cgit v1.2.3 From 128ec447ad43d5a25abcbdda6de01ca7ebc681cf Mon Sep 17 00:00:00 2001 From: Benjamin O'Connell-Armand Date: Sat, 14 Jun 2014 15:50:21 -0400 Subject: Fix various compilation issue --- src/modules/mavlink/mavlink_receiver.cpp | 8 ++++---- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 +++++---- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8a00509a4..c95ee880c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -390,14 +390,14 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message break; } - if (m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES || m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES || ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - } else if (m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { + } else if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { /*Temporary hack to use set_quad_swarm_roll_pitch_yaw_thrust msg for position or velocity */ /* Converts INT16 centimeters to float meters */ @@ -483,10 +483,10 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } if (_global_vel_sp_pub < 0) { - _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint_s), &_global_vel_sp_pub); + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp_pub); } else { - orb_publish(ORB_ID(vehicle_global_velocity_setpoint_s), _global_vel_sp_pub, &global_vel_sp); + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp); } } else if (_control_mode.flag_control_attitude_enabled) { diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 53394b41c..86112fa21 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -117,9 +117,9 @@ private: int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ - int _pos_sp_triplet_sub; /**< position setpoint triplet */ - int _local_pos_sp_sub; /**< offboard local position setpoint */ - int _global_vel_sp_sub; /**< offboard global velocity setpoint */ + int _pos_sp_triplet_sub; /**< position setpoint triplet */ + int _local_pos_sp_sub; /**< offboard local position setpoint */ + int _global_vel_sp_sub; /**< offboard global velocity setpoint */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ @@ -802,6 +802,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) { _vel_sp(0) = _global_vel_sp.vx; _vel_sp(1) = _global_vel_sp.vy; + } } if (!_control_mode.flag_control_manual_enabled) { @@ -827,7 +828,7 @@ MulticopterPositionControl::task_main() /* Close fd to let offboard vel sp be advertised in mavlink receiver */ if (_control_mode.flag_control_offboard_enabled && _global_vel_sp_pub > 0) { - close (_global_vel_sp); + close(_global_vel_sp_pub); _global_vel_sp_pub = -1; } -- cgit v1.2.3 From 5be741607c0d8d477ff30c7639edbd3bce427e7b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 14 Jun 2014 23:57:29 +0200 Subject: mavlink: mission manager moved to separate class and reworked --- src/modules/commander/commander.cpp | 25 +- src/modules/mavlink/mavlink_main.cpp | 766 +++------------------------- src/modules/mavlink/mavlink_main.h | 92 +--- src/modules/mavlink/mavlink_mission.cpp | 823 +++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_mission.h | 176 +++++++ src/modules/mavlink/mavlink_receiver.cpp | 13 +- src/modules/mavlink/mavlink_receiver.h | 2 - src/modules/mavlink/module.mk | 1 + src/modules/mavlink/util.h | 53 -- src/modules/mavlink/waypoints.h | 111 ----- src/modules/navigator/mission.cpp | 107 ++-- src/modules/navigator/mission.h | 2 + src/modules/uORB/topics/mission.h | 6 +- src/modules/uORB/topics/mission_result.h | 8 +- 14 files changed, 1168 insertions(+), 1017 deletions(-) create mode 100644 src/modules/mavlink/mavlink_mission.cpp create mode 100644 src/modules/mavlink/mavlink_mission.h delete mode 100644 src/modules/mavlink/util.h delete mode 100644 src/modules/mavlink/waypoints.h diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d2f8c2994..379ce45fb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -715,17 +715,27 @@ int commander_thread_main(int argc, char *argv[]) struct home_position_s home; memset(&home, 0, sizeof(home)); - /* init mission state */ + /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ + orb_advert_t mission_pub = -1; mission_s mission; if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { - mavlink_log_info(mavlink_fd, "[cmd] dataman ID: %i, count: %u, current: %i", - mission.dataman_id, mission.count, mission.current_index); - orb_advert_t mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); - close(mission_pub); + warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); + mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", + mission.dataman_id, mission.count, mission.current_seq); } else { + warnx("reading mission state failed"); mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + + /* initialize mission state in dataman */ + mission.dataman_id = 0; + mission.count = 0; + mission.current_seq = 0; + dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); } + + mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + orb_publish(ORB_ID(offboard_mission), mission_pub, &mission); } mavlink_log_info(mavlink_fd, "[cmd] started"); @@ -1310,7 +1320,7 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { + if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.finished)) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { @@ -1327,7 +1337,7 @@ int commander_thread_main(int argc, char *argv[]) /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { + mission_result.finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { status.failsafe_state = FAILSAFE_STATE_RC_LOSS; @@ -1488,6 +1498,7 @@ int commander_thread_main(int argc, char *argv[]) close(diff_pres_sub); close(param_changed_sub); close(battery_sub); + close(mission_pub); thread_running = false; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 126c4dfc6..f8a31a4ad 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -102,6 +102,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); +extern mavlink_system_t mavlink_system; + static uint64_t last_write_success_times[6] = {0}; static uint64_t last_write_try_times[6] = {0}; @@ -200,9 +202,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } } - - - } static void usage(void); @@ -221,6 +220,7 @@ Mavlink::Mavlink() : _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), + _mission_result_sub(-1), _mission_pub(-1), _mode(MAVLINK_MODE_NORMAL), _total_counter(0), @@ -238,12 +238,11 @@ Mavlink::Mavlink() : _param_component_id(0), _param_system_type(0), _param_use_hil_gps(0), + _mission_manager(nullptr), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { - _wpm = &_wpm_s; - fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; _instance_id = Mavlink::instance_count(); @@ -421,7 +420,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) } void -Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) +Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) { Mavlink *inst; @@ -497,7 +496,6 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) void Mavlink::mavlink_update_system(void) { - if (!_param_initialized) { _param_system_id = param_find("MAV_SYS_ID"); _param_component_id = param_find("MAV_COMP_ID"); @@ -701,9 +699,32 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -extern mavlink_system_t mavlink_system; +void +Mavlink::send_message(const mavlink_message_t *msg) +{ + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; -int Mavlink::mavlink_pm_queued_send() + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); + mavlink_send_uart_bytes(_channel, buf, len); +} + +void +Mavlink::handle_message(const mavlink_message_t *msg) +{ + /* handle packet with mission manager */ + _mission_manager->handle_message(msg); + + /* handle packet with parameter component */ + mavlink_pm_message_handler(_channel, msg); + + if (get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(msg, this); + } +} + +int +Mavlink::mavlink_pm_queued_send() { if (_mavlink_param_queue_index < param_count()) { mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); @@ -780,7 +801,7 @@ int Mavlink::mavlink_pm_send_param(param_t param) mavlink_type, param_count(), param_get_index(param)); - mavlink_missionlib_send_message(&tx_msg); + send_message(&tx_msg); return OK; } @@ -794,7 +815,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + send_statustext("[mavlink pm] sending list"); } } break; @@ -818,7 +839,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown: %s", name); - mavlink_missionlib_send_gcs_string(buf); + send_statustext(buf); } else { /* set and send parameter */ @@ -854,677 +875,18 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav } } -void Mavlink::publish_mission() -{ - /* Initialize mission publication if necessary */ - if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(offboard_mission), &_mission); - - } else { - orb_publish(ORB_ID(offboard_mission), _mission_pub, &_mission); - } -} - -int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) -{ - /* only support global waypoints for now */ - switch (mavlink_mission_item->frame) { - case MAV_FRAME_GLOBAL: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = false; - break; - - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = true; - break; - - case MAV_FRAME_LOCAL_NED: - case MAV_FRAME_LOCAL_ENU: - return MAV_MISSION_UNSUPPORTED_FRAME; - - case MAV_FRAME_MISSION: - default: - return MAV_MISSION_ERROR; - } - - switch (mavlink_mission_item->command) { - case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param1; - break; - case MAV_CMD_DO_JUMP: - mission_item->do_jump_mission_index = mavlink_mission_item->param1; - mission_item->do_jump_repeat_count = mavlink_mission_item->param2; - break; - default: - mission_item->acceptance_radius = mavlink_mission_item->param2; - mission_item->time_inside = mavlink_mission_item->param1; - break; - } - - mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); - mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); - mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; - - mission_item->autocontinue = mavlink_mission_item->autocontinue; - // mission_item->index = mavlink_mission_item->seq; - mission_item->origin = ORIGIN_MAVLINK; - - /* reset DO_JUMP count */ - mission_item->do_jump_current_count = 0; - - return OK; -} - -int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) -{ - if (mission_item->altitude_is_relative) { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL; - - } else { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - } - - switch (mission_item->nav_cmd) { - case NAV_CMD_TAKEOFF: - mavlink_mission_item->param1 = mission_item->pitch_min; - break; - - case NAV_CMD_DO_JUMP: - mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; - break; - - default: - mavlink_mission_item->param2 = mission_item->acceptance_radius; - mavlink_mission_item->param1 = mission_item->time_inside; - break; - } - - mavlink_mission_item->x = (float)mission_item->lat; - mavlink_mission_item->y = (float)mission_item->lon; - mavlink_mission_item->z = mission_item->altitude; - - mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; - mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; - mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->autocontinue = mission_item->autocontinue; - // mavlink_mission_item->seq = mission_item->index; - - return OK; -} - -void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) -{ - /* get offboard_mission topic */ - int mission_sub = orb_subscribe(ORB_ID(offboard_mission)); - if (orb_copy(ORB_ID(offboard_mission), mission_sub, &_mission)) { - /* error getting topic, init to safe values */ - _mission.dataman_id = 0; - _mission.count = 0; - _mission.current_index = -1; - } - close(mission_sub); - - /* init WPM state */ - state->size = _mission.count; - state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->dataman_id = _mission.dataman_id; - state->current_state = MAVLINK_WPM_STATE_IDLE; - state->current_partner_sysid = 0; - state->current_partner_compid = 0; - state->current_dataman_id = 0; - state->timestamp_lastaction = 0; - state->timestamp_last_send_setpoint = 0; - state->timestamp_last_send_request = 0; - state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - - warnx("wpm init: dataman_id=%d, count=%u, current=%d", state->dataman_id, state->size, _mission.current_index); -} - -/* - * @brief Sends an waypoint ack message - */ -void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_mission_ack_t wpa; - - wpa.target_system = sysid; - wpa.target_component = compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if (seq < _wpm->size) { - mavlink_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = seq; - - mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - } else if (seq == 0 && _wpm->size == 0) { - /* don't broadcast if no WPs */ - - } else { - if (_verbose) { warnx("WPM: Send WAYPOINT_CURRENT ERROR: seq %u out of bounds", seq); } - - mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - } -} - -void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_mission_count_t wpc; - - wpc.target_system = sysid; - wpc.target_component = compid; - wpc.count = _wpm->size; - - mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send WAYPOINT_COUNT %u to ID %u", wpc.count, wpc.target_system); } -} - -void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - dm_item_t dm_item = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - struct mission_item_s mission_item; - - if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { - - /* create mission_item_s from mavlink_mission_item_t */ - mavlink_mission_item_t wp; - map_mission_item_to_mavlink_mission_item(&mission_item, &wp); - - mavlink_message_t msg; - wp.target_system = sysid; - wp.target_component = compid; - wp.seq = seq; - wp.current = (_mission_result.index_current_mission == seq) ? 1 : 0; - mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send WAYPOINT seq %u to ID %u", wp.seq, wp.target_system); } - - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD"); - - if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, _wpm->dataman_id); } - } -} - -void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < _wpm->max_size) { - mavlink_message_t msg; - mavlink_mission_request_t wpr; - wpr.target_system = sysid; - wpr.target_component = compid; - wpr.seq = seq; - mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr); - mavlink_missionlib_send_message(&msg); - _wpm->timestamp_last_send_request = hrt_absolute_time(); - - if (_verbose) { warnx("WPM: Send WAYPOINT_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } - - } else { - mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - - if (_verbose) { warnx("WPM: Send WAYPINT_REQUEST ERROR: seq %u exceeds list capacity", seq); } - } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_item_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send WAYPOINT_REACHED seq %u", wp_reached.seq); } -} - -void Mavlink::mavlink_waypoint_eventloop(uint64_t now) -{ - /* check for timed-out operations */ - if (now - _wpm->timestamp_lastaction > _wpm->action_timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("Operation timeout"); - - if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_partner_sysid = 0; - _wpm->current_partner_compid = 0; - - } else if (now - _wpm->timestamp_last_send_request > _wpm->retry_timeout && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - /* try to get WP again after short timeout */ - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } -} - - -void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) -{ - uint64_t now = hrt_absolute_time(); - - switch (msg->msgid) { - - case MAVLINK_MSG_ID_MISSION_ACK: { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(msg, &wpa); - - if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { - if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (_wpm->current_wp_id == _wpm->size - 1) { - if (_verbose) { warnx("WPM: MISSION_ACK all items sent, switch to MAVLINK_WPM_STATE_IDLE"); } - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_wp_id = 0; - } - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - - if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { - mavlink_mission_set_current_t wpc; - mavlink_msg_mission_set_current_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.seq < _wpm->size) { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT OK (%d)", wpc.seq); } - - _mission.current_index = wpc.seq; - publish_mission(); - - } else { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); } - - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - } - - } else { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } - - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { - mavlink_mission_request_list_t wprl; - mavlink_msg_mission_request_list_decode(msg, &wprl); - - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (_wpm->size > 0) { - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - - } else { - if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST nothing to send, mission is empty"); } - } - - _wpm->current_count = _wpm->size; - mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count); - - } else { - if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } - - mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - - if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) { - _wpm->timestamp_lastaction = now; - - if (wpr.seq >= _wpm->size) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: ignored, requested seq %u was out of bounds [0 %d]", wpr.seq, _wpm->size - 1); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - break; - } - - /* - * Ensure that we are in the correct state and that the first request has id 0 - * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpr.seq == 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: first id != 0 (%d)", wpr.seq); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - break; - } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq == _wpm->current_wp_id) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u (again) from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - } else if (wpr.seq == _wpm->current_wp_id + 1) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, should be %i or %i", wpr.seq, msg->sysid, _wpm->current_wp_id, _wpm->current_wp_id + 1); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - break; - } - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _wpm->current_state); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - break; - } - - _wpm->current_wp_id = wpr.seq; - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - if (wpr.seq < _wpm->size) { - mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bounds", wpr.seq); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - - if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); - break; - } - - if (wpc.count == 0) { - if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } - - mavlink_missionlib_send_gcs_string("WP COUNT 0: CLEAR MISSION"); - break; - } - - if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - - if (_wpm->current_wp_id == 0) { - /* looks like our WAYPOINT_REQUEST was lost, try again */ - if (_verbose) { warnx("WPM: MISSION_COUNT %u (again) from ID %u", wpc.count, msg->sysid); } - - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - - } else { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _wpm->current_wp_id); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - break; - } - - } else { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _wpm->current_state); } - - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - break; - } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - _wpm->current_count = wpc.count; - _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission - - _mission.current_index = -1; - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } - } - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { - _wpm->timestamp_lastaction = now; - - /* Ensure that we are in the correct state and that the first waypoint has id 0 - * and the following waypoints have the correct ids - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (wp.seq != 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq != 0 (%d)", wp.seq); } - break; - } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (wp.seq >= _wpm->current_count) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq %d out of bounds [0 %d]", wp.seq, _wpm->current_count - 1); } - - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - break; - } - - if (wp.seq != _wpm->current_wp_id) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _wpm->current_wp_id); } - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - break; - } - } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - - struct mission_item_s mission_item; - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); - - if (ret != OK) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - dm_item_t dm_item = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - - if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _wpm->current_dataman_id); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - /* waypoint marked as current */ - if (wp.current) { - _mission.current_index = wp.seq; - } - - if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } - - _wpm->current_wp_id = wp.seq + 1; - - if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - /* got all new mission items successfully */ - if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - /* update WPM state */ - _wpm->dataman_id = _wpm->current_dataman_id; - _wpm->size = _wpm->current_count; - - /* update mission topic */ - _mission.dataman_id = _wpm->dataman_id; - _mission.count = _wpm->current_count; - - publish_mission(); - - } else { - /* request next item */ - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { - mavlink_mission_clear_all_t wpca; - mavlink_msg_mission_clear_all_decode(msg, &wpca); - - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - /* clear only active dataman storage, don't change storage id */ - dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - - if (dm_clear(dm_id) == OK) { - if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - /* update WPM state */ - _wpm->timestamp_lastaction = now; - _wpm->size = 0; - - /* update mission topic */ - _mission.dataman_id = _wpm->dataman_id; - _mission.count = 0; - _mission.current_index = -1; - - publish_mission(); - - } else { - if (_verbose) { warnx("WPM: CLEAR_ALL ERROR: can't clear dataman mission storage"); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - } - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - - if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } - } - } - - break; - } - - default: { - /* other messages might should get caught by mavlink and others */ - break; - } - } -} - -void -Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) +int +Mavlink::send_statustext(const char *string) { - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - mavlink_send_uart_bytes(_channel, buf, len); + return send_statustext(MAV_SEVERITY_INFO, string); } - int -Mavlink::mavlink_missionlib_send_gcs_string(const char *string) +Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; - statustext.severity = MAV_SEVERITY_INFO; + statustext.severity = severity; int i = 0; @@ -1546,7 +908,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) return OK; } else { - return 1; + return ERROR; } } @@ -1684,7 +1046,7 @@ Mavlink::message_buffer_is_empty() bool -Mavlink::message_buffer_write(void *ptr, int size) +Mavlink::message_buffer_write(const void *ptr, int size) { // bytes available to write int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; @@ -1751,7 +1113,7 @@ Mavlink::message_buffer_mark_read(int n) } void -Mavlink::pass_message(mavlink_message_t *msg) +Mavlink::pass_message(const mavlink_message_t *msg) { if (_passing_on) { /* size is 8 bytes plus variable payload */ @@ -1762,6 +1124,11 @@ Mavlink::pass_message(mavlink_message_t *msg) } } +float +Mavlink::get_rate_mult() +{ + return _datarate / 1000.0f; +} int Mavlink::task_main(int argc, char *argv[]) @@ -1879,8 +1246,6 @@ Mavlink::task_main(int argc, char *argv[]) break; } - _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ @@ -1922,11 +1287,11 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - /* initialize waypoint manager */ - mavlink_wpm_init(_wpm); + _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - memset(&_mission_result, 0, sizeof(_mission_result)); + /* create mission manager */ + _mission_manager = new MavlinkMissionManager(this); + _mission_manager->set_verbose(_verbose); _task_running = true; @@ -1941,7 +1306,7 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkCommandsStream commands_stream(this, _channel); /* add default streams depending on mode and intervals depending on datarate */ - float rate_mult = _datarate / 1000.0f; + float rate_mult = get_rate_mult(); configure_stream("HEARTBEAT", 1.0f); @@ -1976,7 +1341,6 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ @@ -2030,36 +1394,16 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - bool updated; - orb_check(mission_result_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &_mission_result); - - if (_verbose) { warnx("WPM: got mission result, new current: %d", _mission_result.index_current_mission); } - - if (_mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)_mission_result.mission_index_reached); - } - - mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); - - } else { - if (slow_rate_limiter.check(t)) { - mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); - } - } - if (fast_rate_limiter.check(t)) { mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(hrt_absolute_time()); + _mission_manager->eventloop(); if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { - mavlink_missionlib_send_gcs_string(msg.text); + send_statustext(msg.text); } } } @@ -2093,11 +1437,11 @@ Mavlink::task_main(int argc, char *argv[]) } } - - perf_end(_loop_perf); } + delete _mission_manager; + delete _subscribe_to_stream; _subscribe_to_stream = nullptr; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5d7273abd..085a97d73 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -56,51 +56,7 @@ #include "mavlink_orb_subscription.h" #include "mavlink_stream.h" #include "mavlink_messages.h" - -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -#define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds -#define MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - uint16_t size; ///< Count of waypoints in active mission - uint16_t max_size; ///< Maximal count of waypoints - int dataman_id; ///< Dataman storage ID for active mission - enum MAVLINK_WPM_STATES current_state; ///< Current waypoint manager state - int16_t current_wp_id; ///< Waypoint ID in current transmission - uint16_t current_count; ///< Waypoints count in current transmission - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - int current_dataman_id; ///< Dataman storage ID for current transmission - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_last_send_request; - uint32_t action_timeout; - uint32_t retry_timeout; -}; +#include "mavlink_mission.h" class Mavlink @@ -143,7 +99,7 @@ public: static bool instance_exists(const char *device_name, Mavlink *self); - static void forward_message(mavlink_message_t *msg, Mavlink *self); + static void forward_message(const mavlink_message_t *msg, Mavlink *self); static int get_uart_fd(unsigned index); @@ -168,10 +124,6 @@ public: bool get_forwarding_on() { return _forwarding_on; } - /** - * Handle waypoint related messages. - */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); static int start_helper(int argc, char *argv[]); @@ -192,6 +144,10 @@ public: */ int set_hil_enabled(bool hil_enabled); + void send_message(const mavlink_message_t *msg); + + void handle_message(const mavlink_message_t *msg); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); int get_instance_id(); @@ -209,6 +165,10 @@ public: int get_mavlink_fd() { return _mavlink_fd; } + int send_statustext(const char *string); + int send_statustext(enum MAV_SEVERITY severity, const char *string); + + float get_rate_mult(); /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } @@ -238,12 +198,12 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; + MavlinkMissionManager *_mission_manager; + orb_advert_t _mission_pub; - struct mission_s _mission; - struct mission_result_s _mission_result; + int _mission_result_sub; MAVLINK_MODE _mode; - uint8_t _mavlink_wpm_comp_id; mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; @@ -251,10 +211,6 @@ private: pthread_t _receive_thread; - /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; - bool _verbose; bool _forwarding_on; bool _passing_on; @@ -336,20 +292,9 @@ private: void mavlink_update_system(); - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); - void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); - void mavlink_wpm_send_waypoint_current(uint16_t seq); - void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); - void mavlink_wpm_init(mavlink_wpm_storage *state); - int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); - void publish_mission(); - - void mavlink_missionlib_send_message(mavlink_message_t *msg); - int mavlink_missionlib_send_gcs_string(const char *string); + uint8_t get_system_id(); + + uint8_t get_component_id(); int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); @@ -364,13 +309,13 @@ private: int message_buffer_is_empty(); - bool message_buffer_write(void *ptr, int size); + bool message_buffer_write(const void *ptr, int size); int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); - void pass_message(mavlink_message_t *msg); + void pass_message(const mavlink_message_t *msg); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); @@ -378,5 +323,4 @@ private: * Main mavlink task. */ int task_main(int argc, char *argv[]); - }; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp new file mode 100644 index 000000000..1eab7d761 --- /dev/null +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -0,0 +1,823 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_mission.cpp + * MAVLink mission manager implementation. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "mavlink_mission.h" +#include "mavlink_main.h" + +#include +#include +#include +#include + +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + + +MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : + _mavlink(mavlink), + _channel(mavlink->get_channel()), + _comp_id(MAV_COMP_ID_MISSIONPLANNER), + _state(MAVLINK_WPM_STATE_IDLE), + _time_last_recv(0), + _time_last_sent(0), + _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), + _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), + _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), + _count(0), + _current_seq(0), + _dataman_id(0), + _transfer_dataman_id(0), + _transfer_count(0), + _transfer_seq(0), + _transfer_current_seq(0), + _transfer_partner_sysid(0), + _transfer_partner_compid(0), + _offboard_mission_sub(-1), + _mission_result_sub(-1), + _offboard_mission_pub(-1), + _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), + _verbose(false) +{ + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + + init_offboard_mission(); +} + +MavlinkMissionManager::~MavlinkMissionManager() +{ + close(_offboard_mission_pub); + close(_mission_result_sub); +} + +void +MavlinkMissionManager::init_offboard_mission() +{ + mission_s mission_state; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) { + _dataman_id = mission_state.dataman_id; + _count = mission_state.count; + _current_seq = mission_state.current_seq; + + warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq); + + } else { + _dataman_id = 0; + _count = 0; + _current_seq = 0; + warnx("offboard mission init: ERROR, reading mission state failed"); + } +} + +/** + * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes. + */ +int +MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq) +{ + struct mission_s mission; + + mission.dataman_id = dataman_id; + mission.count = count; + mission.current_seq = seq; + + /* update mission state in dataman */ + /* lock MISSION_STATE item */ + dm_lock(DM_KEY_MISSION_STATE); + + int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + + /* unlock MISSION_STATE item */ + dm_unlock(DM_KEY_MISSION_STATE); + + if (res == sizeof(mission_s)) { + /* update active mission state */ + _dataman_id = dataman_id; + _count = count; + _current_seq = seq; + + /* mission state saved successfully, publish offboard_mission topic */ + if (_offboard_mission_pub < 0) { + _offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + + } else { + orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission); + } + + return OK; + + } else { + warnx("ERROR: can't save mission state"); + _mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state"); + + return ERROR; + } +} + +void +MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_mission_ack_t wpa; + + wpa.target_system = sysid; + wpa.target_component = compid; + wpa.type = type; + + mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } +} + + +void +MavlinkMissionManager::send_mission_current(uint16_t seq) +{ + if (seq < _count) { + mavlink_message_t msg; + mavlink_mission_current_t wpc; + + wpc.seq = seq; + + mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); + _mavlink->send_message(&msg); + + } else if (seq == 0 && _count == 0) { + /* don't broadcast if no WPs */ + + } else { + if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); } + + _mavlink->send_statustext("ERROR: wp index out of bounds"); + } +} + + +void +MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + _time_last_sent = hrt_absolute_time(); + + mavlink_message_t msg; + mavlink_mission_count_t wpc; + + wpc.target_system = sysid; + wpc.target_component = compid; + wpc.count = _count; + + mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } +} + + +void +MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + dm_item_t dm_item = _dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + struct mission_item_s mission_item; + + if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + _time_last_sent = hrt_absolute_time(); + + /* create mission_item_s from mavlink_mission_item_t */ + mavlink_mission_item_t wp; + format_mavlink_mission_item(&mission_item, &wp); + + mavlink_message_t msg; + wp.target_system = sysid; + wp.target_component = compid; + wp.seq = seq; + wp.current = (_current_seq == seq) ? 1 : 0; + mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("#audio: Unable to read from micro SD"); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } + } +} + + +void +MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < _max_count) { + _time_last_sent = hrt_absolute_time(); + + mavlink_message_t msg; + mavlink_mission_request_t wpr; + wpr.target_system = sysid; + wpr.target_component = compid; + wpr.seq = seq; + mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } + + } else { + _mavlink->send_statustext("ERROR: Waypoint index exceeds list capacity"); + + if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); } + } +} + + +void +MavlinkMissionManager::send_mission_item_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_mission_item_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } +} + + +void +MavlinkMissionManager::eventloop() +{ + hrt_abstime now = hrt_absolute_time(); + + bool updated = false; + orb_check(_mission_result_sub, &updated); + + if (updated) { + mission_result_s mission_result; + orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result); + + _current_seq = mission_result.seq_current; + + if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); } + + if (mission_result.reached) { + send_mission_item_reached((uint16_t)mission_result.seq_reached); + } + + send_mission_current(_current_seq); + + } else { + if (_slow_rate_limiter.check(now)) { + send_mission_current(_current_seq); + } + } + + /* check for timed-out operations */ + if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) { + _mavlink->send_statustext("Operation timeout"); + + if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); } + + _state = MAVLINK_WPM_STATE_IDLE; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + /* try to request item again after timeout */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + + } else if (_state == MAVLINK_WPM_STATE_SENDLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + if (_transfer_seq == 0) { + /* try to send items count again after timeout */ + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count); + + } else { + /* try to send item again after timeout */ + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1); + } + } +} + + +void +MavlinkMissionManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_MISSION_ACK: + handle_mission_ack(msg); + break; + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: + handle_mission_set_current(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: + handle_mission_request_list(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST: + handle_mission_request(msg); + break; + + case MAVLINK_MSG_ID_MISSION_COUNT: + handle_mission_count(msg); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: + handle_mission_item(msg); + break; + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: + handle_mission_clear_all(msg); + break; + + default: + break; + } +} + + +void +MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) +{ + mavlink_mission_ack_t wpa; + mavlink_msg_mission_ack_decode(msg, &wpa); + + if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == _count) { + if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); } + + } else { + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); } + } + + _state = MAVLINK_WPM_STATE_IDLE; + } + + } else { + _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } + } + } +} + + +void +MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) +{ + mavlink_mission_set_current_t wpc; + mavlink_msg_mission_set_current_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.seq < _count) { + if (update_active_mission(_dataman_id, _count, wpc.seq) == OK) { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } + } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); } + + _mavlink->send_statustext("IGN WP CURR CMD: Not in list"); + } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } + + _mavlink->send_statustext("IGN WP CURR CMD: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) +{ + mavlink_mission_request_list_t wprl; + mavlink_msg_mission_request_list_decode(msg, &wprl); + + if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_count > 0) { + _state = MAVLINK_WPM_STATE_SENDLIST; + _transfer_seq = 0; + _transfer_count = _count; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } + + } else { + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } + } + + send_mission_count(msg->sysid, msg->compid, _count); + + } else { + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } + + _mavlink->send_statustext("IGN REQUEST LIST: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) +{ + mavlink_mission_request_t wpr; + mavlink_msg_mission_request_decode(msg, &wpr); + + if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + /* _transfer_seq contains sequence of expected request */ + if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u", wpr.seq, msg->sysid); } + + _transfer_seq++; + + } else if (wpr.seq == _transfer_seq - 1 && wpr.seq >= 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); } + + } else { + if (_transfer_seq > 0 && _transfer_seq < _transfer_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, _transfer_seq - 1, _transfer_seq); } + + } else if (_transfer_seq <= 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq); } + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); } + } + + _state = MAVLINK_WPM_STATE_IDLE; + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + return; + } + + /* double check bounds in case of items count changed */ + if (wpr.seq >= 0 && wpr.seq < _count) { + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [0, %u]", wpr.seq, msg->sysid, wpr.seq, _count - 1); } + + _state = MAVLINK_WPM_STATE_IDLE; + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); } + + _mavlink->send_statustext("IGN MISSION_ITEM_REQUEST: No transfer"); + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); } + + _mavlink->send_statustext("REJ. WP CMD: Busy"); + } + + } else { + _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } + } + } +} + + +void +MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) +{ + mavlink_mission_count_t wpc; + mavlink_msg_mission_count_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.count > _max_count) { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE); + return; + } + + if (wpc.count == 0) { + if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } + + /* alternate dataman ID anyway to let navigator know about changes */ + update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); + _mavlink->send_statustext("WP COUNT 0: CLEAR MISSION"); + + // TODO send ACK? + return; + } + + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } + + _state = MAVLINK_WPM_STATE_GETLIST; + _transfer_seq = 0; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + _transfer_count = wpc.count; + _transfer_dataman_id = _dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + _transfer_current_seq = -1; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == 0) { + /* looks like our MISSION_REQUEST was lost, try again */ + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); } + + _mavlink->send_statustext("WP CMD OK AGAIN"); + + } else { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); } + + _mavlink->send_statustext("REJ. WP CMD: Busy"); + return; + } + + } else { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); } + + _mavlink->send_statustext("IGN MISSION_COUNT: Busy"); + return; + } + + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } +} + + +void +MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) +{ + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); + + if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) { + if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (wp.seq != _transfer_seq) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); } + + /* don't send request here, it will be performed in eventloop after timeout */ + return; + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); } + + _mavlink->send_statustext("IGN MISSION_ITEM: No transfer"); + return; + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } + + _mavlink->send_statustext("IGN MISSION_ITEM: Busy"); + return; + } + + struct mission_item_s mission_item; + int ret = parse_mavlink_mission_item(&wp, &mission_item); + + if (ret != OK) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); + _state = MAVLINK_WPM_STATE_IDLE; + return; + } + + dm_item_t dm_item = _transfer_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + + if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("#audio: Unable to write on micro SD"); + _state = MAVLINK_WPM_STATE_IDLE; + return; + } + + /* waypoint marked as current */ + if (wp.current) { + _transfer_current_seq = wp.seq; + } + + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } + + _transfer_seq = wp.seq + 1; + + if (_transfer_seq == _transfer_count) { + /* got all new mission items successfully */ + if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } + + _state = MAVLINK_WPM_STATE_IDLE; + + if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + /* request next item */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } + } +} + + +void +MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) +{ + mavlink_mission_clear_all_t wpca; + mavlink_msg_mission_clear_all_decode(msg, &wpca); + + if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { + + if (_state == MAVLINK_WPM_STATE_IDLE) { + /* don't touch mission items storage itself, but only items count in mission state */ + _time_last_recv = hrt_absolute_time(); + + if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) { + if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + _mavlink->send_statustext("IGN WP CLEAR CMD: Busy"); + + if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } + } + } +} + +int +MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +{ + /* only support global waypoints for now */ + switch (mavlink_mission_item->frame) { + case MAV_FRAME_GLOBAL: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = false; + break; + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = true; + break; + + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_ENU: + return MAV_MISSION_UNSUPPORTED_FRAME; + + case MAV_FRAME_MISSION: + default: + return MAV_MISSION_ERROR; + } + + switch (mavlink_mission_item->command) { + case MAV_CMD_NAV_TAKEOFF: + mission_item->pitch_min = mavlink_mission_item->param1; + break; + case MAV_CMD_DO_JUMP: + mission_item->do_jump_mission_index = mavlink_mission_item->param1; + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; + break; + default: + mission_item->acceptance_radius = mavlink_mission_item->param2; + mission_item->time_inside = mavlink_mission_item->param1; + break; + } + + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + + mission_item->autocontinue = mavlink_mission_item->autocontinue; + // mission_item->index = mavlink_mission_item->seq; + mission_item->origin = ORIGIN_MAVLINK; + + /* reset DO_JUMP count */ + mission_item->do_jump_current_count = 0; + + return MAV_MISSION_ACCEPTED; +} + + +int +MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) +{ + if (mission_item->altitude_is_relative) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + + switch (mission_item->nav_cmd) { + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param1 = mission_item->pitch_min; + break; + + case NAV_CMD_DO_JUMP: + mavlink_mission_item->param1 = mission_item->do_jump_mission_index; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + break; + + default: + mavlink_mission_item->param2 = mission_item->acceptance_radius; + mavlink_mission_item->param1 = mission_item->time_inside; + break; + } + + mavlink_mission_item->x = (float)mission_item->lat; + mavlink_mission_item->y = (float)mission_item->lon; + mavlink_mission_item->z = mission_item->altitude; + + mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; + mavlink_mission_item->command = mission_item->nav_cmd; + mavlink_mission_item->autocontinue = mission_item->autocontinue; + // mavlink_mission_item->seq = mission_item->index; + + return OK; +} diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h new file mode 100644 index 000000000..d8695cf83 --- /dev/null +++ b/src/modules/mavlink/mavlink_mission.h @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_mission.h + * MAVLink mission manager interface definition. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "mavlink_bridge_header.h" +#include "mavlink_rate_limiter.h" +#include + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + +#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds +#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds + +class Mavlink; + +class MavlinkMissionManager { +public: + MavlinkMissionManager(Mavlink *parent); + + ~MavlinkMissionManager(); + + void init_offboard_mission(); + + int update_active_mission(int dataman_id, unsigned count, int seq); + + void send_message(mavlink_message_t *msg); + + /** + * @brief Sends an waypoint ack message + */ + void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type); + + /** + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ + void send_mission_current(uint16_t seq); + + void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count); + + void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq); + + void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq); + + /** + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ + void send_mission_item_reached(uint16_t seq); + + void eventloop(); + + void handle_message(const mavlink_message_t *msg); + + void handle_mission_ack(const mavlink_message_t *msg); + + void handle_mission_set_current(const mavlink_message_t *msg); + + void handle_mission_request_list(const mavlink_message_t *msg); + + void handle_mission_request(const mavlink_message_t *msg); + + void handle_mission_count(const mavlink_message_t *msg); + + void handle_mission_item(const mavlink_message_t *msg); + + void handle_mission_clear_all(const mavlink_message_t *msg); + + /** + * Parse mavlink MISSION_ITEM message to get mission_item_s. + */ + int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); + + /** + * Format mission_item_s as mavlink MISSION_ITEM message. + */ + int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); + + void set_verbose(bool v) { _verbose = v; } + +private: + Mavlink* _mavlink; + mavlink_channel_t _channel; + uint8_t _comp_id; + + enum MAVLINK_WPM_STATES _state; ///< Current state + + uint64_t _time_last_recv; + uint64_t _time_last_sent; + + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximal count of mission items + + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission + + int _transfer_dataman_id; ///< Dataman storage ID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + int _transfer_seq; ///< Item sequence in current transmission + int _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission + uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission + + int _offboard_mission_sub; + int _mission_result_sub; + orb_advert_t _offboard_mission_pub; + + MavlinkRateLimiter _slow_rate_limiter; + + bool _verbose; +}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1e217ec70..834852d6f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -79,7 +79,6 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" #include "mavlink_receiver.h" #include "mavlink_main.h" -#include "util.h" __END_DECLS @@ -902,16 +901,8 @@ MavlinkReceiver::receive_thread(void *arg) /* handle generic messages and commands */ handle_message(&msg); - /* handle packet with waypoint component */ - _mavlink->mavlink_wpm_message_handler(&msg); - - /* handle packet with parameter component */ - _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); - - if (_mavlink->get_forwarding_on()) { - /* forward any messages to other mavlink instances */ - Mavlink::forward_message(&msg, _mavlink); - } + /* handle packet with parent object */ + _mavlink->handle_message(&msg); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..df5d037f8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -100,8 +100,6 @@ public: static void *start_helper(void *context); private: - perf_counter_t _loop_perf; /**< loop performance counter */ - Mavlink *_mavlink; void handle_message(mavlink_message_t *msg); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f532e26fe..725a9e184 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,6 +39,7 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ + mavlink_mission.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h deleted file mode 100644 index 5ca9a085d..000000000 --- a/src/modules/mavlink/util.h +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file util.h - * Utility and helper functions and data. - */ - -#pragma once - -/** MAVLink communications channel */ -extern uint8_t chan; - -/** Shutdown marker */ -extern volatile bool thread_should_exit; - -/** Waypoint storage */ -extern mavlink_wpm_storage *wpm; - -/** - * Translate the custom state into standard mavlink modes and state. - */ -extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h deleted file mode 100644 index a2109f2d8..000000000 --- a/src/modules/mavlink/waypoints.h +++ /dev/null @@ -1,111 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2009-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file waypoints.h - * MAVLink waypoint protocol definition (BSD-relicensed). - * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - */ - -#ifndef WAYPOINTS_H_ -#define WAYPOINTS_H_ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - -#include -#include "mavlink_bridge_header.h" -#include -#include - -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -#define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - uint16_t size; - uint16_t max_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint32_t action_timeout; - int current_dataman_id; -}; - -typedef struct mavlink_wpm_storage mavlink_wpm_storage; - -int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); -int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - - -void mavlink_wpm_init(mavlink_wpm_storage *state); -void mavlink_waypoint_eventloop(uint64_t now); -void mavlink_wpm_message_handler(const mavlink_message_t *msg); - -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); - -static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; - -#endif /* WAYPOINTS_H_ */ diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index d9d8353f6..f6d304c37 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -67,13 +67,11 @@ Mission::Mission(Navigator *navigator, const char *name) : _current_offboard_mission_index(-1), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE) + _mission_type(MISSION_TYPE_NONE), + _inited(false) { /* load initial params */ updateParams(); - /* set initial mission items */ - on_inactive(); - } Mission::~Mission() @@ -85,16 +83,25 @@ Mission::on_inactive() { _first_run = true; - /* check anyway if missions have changed so that feedback to groundstation is given */ - bool onboard_updated; - orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); - if (onboard_updated) { - update_onboard_mission(); - } + if (_inited) { + /* check if missions have changed so that feedback to ground station is given */ + bool onboard_updated = false; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } - bool offboard_updated; - orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); - if (offboard_updated) { + bool offboard_updated = false; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } + + } else { + /* read mission topics on initialization */ + _inited = true; + + update_onboard_mission(); update_offboard_mission(); } } @@ -105,13 +112,13 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) bool updated = false; /* check if anything has changed */ - bool onboard_updated; + bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); if (onboard_updated) { update_onboard_mission(); } - bool offboard_updated; + bool offboard_updated = false; orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); if (offboard_updated) { update_offboard_mission(); @@ -139,9 +146,9 @@ Mission::update_onboard_mission() { if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { /* accept the current index set by the onboard mission if it is within bounds */ - if (_onboard_mission.current_index >=0 - && _onboard_mission.current_index < (int)_onboard_mission.count) { - _current_onboard_mission_index = _onboard_mission.current_index; + if (_onboard_mission.current_seq >=0 + && _onboard_mission.current_seq < (int)_onboard_mission.count) { + _current_onboard_mission_index = _onboard_mission.current_seq; } else { /* if less WPs available, reset to first WP */ if (_current_onboard_mission_index >= (int)_onboard_mission.count) { @@ -154,7 +161,7 @@ Mission::update_onboard_mission() } } else { _onboard_mission.count = 0; - _onboard_mission.current_index = 0; + _onboard_mission.current_seq = 0; _current_onboard_mission_index = 0; } } @@ -163,13 +170,12 @@ void Mission::update_offboard_mission() { if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { - + warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq); /* determine current index */ - if (_offboard_mission.current_index >= 0 - && _offboard_mission.current_index < (int)_offboard_mission.count) { - _current_offboard_mission_index = _offboard_mission.current_index; + if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) { + _current_offboard_mission_index = _offboard_mission.current_seq; } else { - /* if less WPs available, reset to first WP */ + /* if less items available, reset to first item */ if (_current_offboard_mission_index >= (int)_offboard_mission.count) { _current_offboard_mission_index = 0; /* if not initialized, set it to 0 */ @@ -194,10 +200,12 @@ Mission::update_offboard_mission() _navigator->get_geofence(), _navigator->get_home_position()->alt); } else { + warnx("offboard mission update failed"); _offboard_mission.count = 0; - _offboard_mission.current_index = 0; + _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; } + report_current_offboard_mission_item(); } @@ -326,10 +334,10 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren void Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) { - int next_temp_mission_index = _onboard_mission.current_index + 1; + int next_temp_mission_index = _onboard_mission.current_seq + 1; /* try if there is a next onboard mission */ - if (_onboard_mission.current_index >= 0 && + if (_onboard_mission.current_seq >= 0 && next_temp_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { @@ -349,9 +357,9 @@ void Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) { /* try if there is a next offboard mission */ - int next_temp_mission_index = _offboard_mission.current_index + 1; + int next_temp_mission_index = _offboard_mission.current_seq + 1; warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); - if (_offboard_mission.current_index >= 0 && + if (_offboard_mission.current_seq >= 0 && next_temp_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { @@ -441,19 +449,31 @@ Mission::save_offboard_mission_state() /* read current state */ int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); - /* check if state actually changed to save flash write cycles */ - if (read_res != sizeof(mission_s) || mission_state.dataman_id != _offboard_mission.dataman_id || - mission_state.count != _offboard_mission.count || - mission_state.current_index != _current_offboard_mission_index) { + if (read_res == sizeof(mission_s)) { + /* data read successfully, check dataman ID and items count */ + if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) { + /* navigator may modify only sequence, write modified state only if it changed */ + if (mission_state.current_seq != _current_offboard_mission_index) { + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + warnx("ERROR: can't save mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state"); + } + } + } + } else { + /* invalid data, this must not happen and indicates error in offboard_mission publisher */ mission_state.dataman_id = _offboard_mission.dataman_id; mission_state.count = _offboard_mission.count; - mission_state.current_index = _current_offboard_mission_index; + mission_state.current_seq = _current_offboard_mission_index; - /* write modifyed state only if changed */ - if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "error saving mission state"); + warnx("ERROR: invalid mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state"); + /* write modified state only if changed */ + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + warnx("ERROR: can't save mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state"); } } @@ -465,8 +485,8 @@ void Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; + _mission_result.reached = true; + _mission_result.seq_reached = _current_offboard_mission_index; } publish_mission_result(); } @@ -474,7 +494,8 @@ Mission::report_mission_item_reached() void Mission::report_current_offboard_mission_item() { - _mission_result.index_current_mission = _current_offboard_mission_index; + warnx("current offboard mission index: %d", _current_offboard_mission_index); + _mission_result.seq_current = _current_offboard_mission_index; publish_mission_result(); save_offboard_mission_state(); @@ -483,7 +504,7 @@ Mission::report_current_offboard_mission_item() void Mission::report_mission_finished() { - _mission_result.mission_finished = true; + _mission_result.finished = true; publish_mission_result(); } @@ -500,6 +521,6 @@ Mission::publish_mission_result() _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } /* reset reached bool */ - _mission_result.mission_reached = false; - _mission_result.mission_finished = false; + _mission_result.reached = false; + _mission_result.finished = false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 98b11bade..1310671b0 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -183,6 +183,8 @@ private: MISSION_TYPE_OFFBOARD } _mission_type; + bool _inited; + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 4bcaaaa5a..499b2e1e5 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -97,11 +97,15 @@ struct mission_item_s { unsigned do_jump_current_count; /**< count how many times the jump has been done */ }; +/** + * This topic used to notify navigator about mission changes, mission itself and new mission state + * must be stored in dataman before publication. + */ struct mission_s { int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ unsigned count; /**< count of the missions stored in the dataman */ - int current_index; /**< default -1, start at the one changed latest */ + int current_seq; /**< default -1, start at the one changed latest */ }; /** diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index ad654a9ff..beb797e62 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -53,10 +53,10 @@ struct mission_result_s { - bool mission_reached; /**< true if mission has been reached */ - unsigned mission_index_reached; /**< index of the mission which has been reached */ - unsigned index_current_mission; /**< index of the current mission */ - bool mission_finished; /**< true if mission has been completed */ + unsigned seq_reached; /**< Sequence of the mission item which has been reached */ + unsigned seq_current; /**< Sequence of the current mission item */ + bool reached; /**< true if mission has been reached */ + bool finished; /**< true if mission has been completed */ }; /** -- cgit v1.2.3 From 74ae18c245514d918e6ea58f05de98b9fe4b63a5 Mon Sep 17 00:00:00 2001 From: Benjamin O'Connell-Armand Date: Sat, 14 Jun 2014 18:46:18 -0400 Subject: Fixed rates flags --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2c36b2e6c..7baaf7139 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1834,7 +1834,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = true; break; case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: - control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; -- cgit v1.2.3 From de5c3580b341759bbc4c35bba46d34033e02df10 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Jun 2014 10:56:07 +0200 Subject: adding individual warnings for all defined reset conditions --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 70c1cddb5..501b7cc1b 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -997,6 +997,20 @@ FixedwingEstimator::task_main() mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); break; } + case 5: + { + const char* str = "diverged too far from GPS"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 6: + { + const char* str = "excessive covariances"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } default: { -- cgit v1.2.3 From 9772aa5814eb2f0d864e89814f5be2cf9f136eda Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 15 Jun 2014 11:59:45 +0200 Subject: mavlink: set current DO_JUMP repetitions to 0 initially --- src/modules/mavlink/mavlink_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3a656ae4d..d7cf45351 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -900,6 +900,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item break; case MAV_CMD_DO_JUMP: mission_item->do_jump_mission_index = mavlink_mission_item->param1; + mission_item->do_jump_current_count = 0; mission_item->do_jump_repeat_count = mavlink_mission_item->param2; break; default: -- cgit v1.2.3 From bd8b071875164f56eb92c8cdc5b4fca92bd7e362 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Jun 2014 12:15:25 +0200 Subject: att pos estimator: on reset use projected gps position instead of [0,0,0] to set position state --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index c2282fbb9..45790063f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1504,7 +1504,7 @@ void AttPosEKF::FuseAirspeed() SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; - + float H_TAS[n_states]; for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f; H_TAS[4] = SH_TAS[2]; @@ -2327,7 +2327,7 @@ int AttPosEKF::CheckAndBound() if (StatesNaN(&last_ekf_error)) { ekf_debug("re-initializing dynamic"); - InitializeDynamic(velNED, magDeclination); + InitializeDynamic(velNED, magDeclination); ret = 1; } @@ -2474,10 +2474,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) magstate.R_MAG = sq(magMeasurementSigma); magstate.DCM = DCM; + // Calculate position from gps measurement + float posNEDInit[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m) + calcposNED(posNEDInit, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities - for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel + // positions: + states[7] = posNEDInit[0]; + states[8] = posNEDInit[1]; + states[9] = posNEDInit[2]; + for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel states[16] = initMagNED.x; // Magnetic Field North states[17] = initMagNED.y; // Magnetic Field East states[18] = initMagNED.z; // Magnetic Field Down -- cgit v1.2.3 From 0700a4f6b823b77ad7b343c641ec8fb34f130173 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Jun 2014 14:46:30 +0200 Subject: att pos estimator: correctly initialize fusionModeGPS variable, this makes the system use gps again and hence fixes the huge drift problems --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 45790063f..d364facad 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -14,6 +14,7 @@ AttPosEKF::AttPosEKF() summedDelAng.zero(); summedDelVel.zero(); + fusionModeGPS = 0; fuseVelData = false; fusePosData = false; fuseHgtData = false; -- cgit v1.2.3 From 1b8e72be1b9614bff4e1bdf81ad7f3265754e062 Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 15 Jun 2014 17:26:14 +0200 Subject: Merge sAcc & pAcc scaling fix from branch inav_gps_delay --- src/drivers/gps/ubx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 1783ae0f7..56e7a8bd7 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -643,8 +643,8 @@ UBX::payload_rx_done(void) UBX_TRACE_RXMSG("Rx NAV-SOL\n"); _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; - _gps_position->s_variance_m_s = _buf.payload_rx_nav_sol.sAcc; - _gps_position->p_variance_m = _buf.payload_rx_nav_sol.pAcc; + _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m + _gps_position->p_variance_m = (float)_buf.payload_rx_nav_sol.pAcc * 1e-2f; // from cm to m _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; _gps_position->timestamp_variance = hrt_absolute_time(); -- cgit v1.2.3 From 243db01a37d5d3adecc2e010ac4d9819cf3d6c37 Mon Sep 17 00:00:00 2001 From: Kynos Date: Mon, 16 Jun 2014 01:43:44 +0200 Subject: Request and display MON-VER message at startup --- src/drivers/gps/ubx.cpp | 73 +++++++++++++++++++++++++++++++++++++++++++------ src/drivers/gps/ubx.h | 21 ++++++++++++-- 2 files changed, 84 insertions(+), 10 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 56e7a8bd7..be19ce488 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -93,6 +93,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _ack_waiting_msg(0) { decode_init(); + memset(_ubx_sw_version, 0, sizeof(_ubx_sw_version)); + memset(_ubx_hw_version, 0, sizeof(_ubx_hw_version)); } UBX::~UBX() @@ -221,6 +223,9 @@ UBX::configure(unsigned &baudrate) return 1; } + /* request module version information by sending an empty MON-VER message */ + send_message(UBX_MSG_MON_VER, nullptr, 0); + _configured = true; return 0; } @@ -385,10 +390,16 @@ UBX::parse_char(const uint8_t b) case UBX_DECODE_PAYLOAD: UBX_TRACE_PARSER("."); add_byte_to_checksum(b); - if (_rx_msg != UBX_MSG_NAV_SVINFO) { - ret = payload_rx_add(b); // add a payload byte - } else { - ret = payload_rx_add_svinfo(b); // add a svinfo payload byte + switch (_rx_msg) { + case UBX_MSG_NAV_SVINFO: + ret = payload_rx_add_nav_svinfo(b); // add a NAV-SVINFO payload byte + break; + case UBX_MSG_MON_VER: + ret = payload_rx_add_mon_ver(b); // add a MON-VER payload byte + break; + default: + ret = payload_rx_add(b); // add a payload byte + break; } if (ret < 0) { // payload not handled, discard message @@ -477,6 +488,9 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured break; + case UBX_MSG_MON_VER: + break; // unconditionally handle this message + case UBX_MSG_MON_HW: if ( (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */ && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) /* u-blox 7+ msg format */ @@ -559,10 +573,10 @@ UBX::payload_rx_add(const uint8_t b) } /** - * Add svinfo payload rx byte + * Add NAV-SVINFO payload rx byte */ int // -1 = error, 0 = ok, 1 = payload completed -UBX::payload_rx_add_svinfo(const uint8_t b) +UBX::payload_rx_add_nav_svinfo(const uint8_t b) { int ret = 0; @@ -606,6 +620,41 @@ UBX::payload_rx_add_svinfo(const uint8_t b) return ret; } +/** + * Add MON-VER payload rx byte + */ +int // -1 = error, 0 = ok, 1 = payload completed +UBX::payload_rx_add_mon_ver(const uint8_t b) +{ + int ret = 0; + + if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) { + // Fill Part 1 buffer + _buf.raw[_rx_payload_index] = b; + } else { + if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) { + // Part 1 complete: decode Part 1 buffer + strncpy(_ubx_sw_version, (char*)_buf.payload_rx_mon_ver_part1.swVersion, sizeof(_ubx_sw_version)); + strncpy(_ubx_hw_version, (char*)_buf.payload_rx_mon_ver_part1.hwVersion, sizeof(_ubx_hw_version)); + UBX_WARN("VER sw %30s", _ubx_sw_version); + UBX_WARN("VER hw %10s", _ubx_hw_version); + } + // fill Part 2 buffer + unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t); + _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) { + // Part 2 complete: decode Part 2 buffer + UBX_WARN("VER ext %30s", _buf.payload_rx_mon_ver_part2.extension); + } + } + + if (++_rx_payload_index >= _rx_payload_length) { + ret = 1; // payload received completely + } + + return ret; +} + /** * Finish payload rx */ @@ -712,6 +761,12 @@ UBX::payload_rx_done(void) ret = 1; break; + case UBX_MSG_MON_VER: + UBX_TRACE_RXMSG("Rx MON-VER\n"); + + ret = 1; + break; + case UBX_MSG_MON_HW: UBX_TRACE_RXMSG("Rx MON-HW\n"); @@ -813,10 +868,12 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len // Calculate checksum calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes - calc_checksum(payload, length, &checksum); + if (payload != nullptr) + calc_checksum(payload, length, &checksum); // Send message write(_fd, (const void *)&header, sizeof(header)); - write(_fd, (const void *)payload, length); + if (payload != nullptr) + write(_fd, (const void *)payload, length); write(_fd, (const void *)&checksum, sizeof(checksum)); } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 8fd8d4da6..c8c89dfad 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -72,6 +72,7 @@ #define UBX_ID_CFG_MSG 0x01 #define UBX_ID_CFG_RATE 0x08 #define UBX_ID_CFG_NAV5 0x24 +#define UBX_ID_MON_VER 0x04 #define UBX_ID_MON_HW 0x09 /* Message Classes & IDs */ @@ -87,6 +88,7 @@ #define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8) #define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8) #define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) +#define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8) /* TX CFG-PRT message contents */ #define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ @@ -249,6 +251,17 @@ typedef struct { uint32_t pullL; } ubx_payload_rx_mon_hw_ubx7_t; +/* Rx MON-VER Part 1 */ +typedef struct { + uint8_t swVersion[30]; + uint8_t hwVersion[10]; +} ubx_payload_rx_mon_ver_part1_t; + +/* Rx MON-VER Part 2 (repeated) */ +typedef struct { + uint8_t extension[30]; +} ubx_payload_rx_mon_ver_part2_t; + /* Rx ACK-ACK */ typedef union { uint16_t msg; @@ -333,6 +346,8 @@ typedef union { ubx_payload_rx_nav_velned_t payload_rx_nav_velned; ubx_payload_rx_mon_hw_ubx6_t payload_rx_mon_hw_ubx6; ubx_payload_rx_mon_hw_ubx7_t payload_rx_mon_hw_ubx7; + ubx_payload_rx_mon_ver_part1_t payload_rx_mon_ver_part1; + ubx_payload_rx_mon_ver_part2_t payload_rx_mon_ver_part2; ubx_payload_rx_ack_ack_t payload_rx_ack_ack; ubx_payload_rx_ack_nak_t payload_rx_ack_nak; ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt; @@ -366,7 +381,6 @@ typedef enum { UBX_RXMSG_ERROR_LENGTH } ubx_rxmsg_state_t; - /* ACK state */ typedef enum { UBX_ACK_IDLE = 0, @@ -400,7 +414,8 @@ private: * Add payload rx byte */ int payload_rx_add(const uint8_t b); - int payload_rx_add_svinfo(const uint8_t b); + int payload_rx_add_nav_svinfo(const uint8_t b); + int payload_rx_add_mon_ver(const uint8_t b); /** * Finish payload rx @@ -455,6 +470,8 @@ private: hrt_abstime _disable_cmd_last; uint16_t _ack_waiting_msg; ubx_buf_t _buf; + char _ubx_sw_version[30]; + char _ubx_hw_version[10]; }; #endif /* UBX_H_ */ -- cgit v1.2.3 From 0ffb123701c2e6124d7c735cdb55c250564d12c6 Mon Sep 17 00:00:00 2001 From: Kynos Date: Mon, 16 Jun 2014 15:35:38 +0200 Subject: Store hash instead of full strings for SW & HW version --- src/drivers/gps/ubx.cpp | 48 +++++++++++++++++++++++++++++++++++++++--------- src/drivers/gps/ubx.h | 8 ++++++-- 2 files changed, 45 insertions(+), 11 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index be19ce488..adc7130dd 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -72,6 +72,10 @@ #define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) #define SWAP16(X) ((((X) >> 8) & 0x00ff) | (((X) << 8) & 0xff00)) +#define FNV1_32_INIT ((uint32_t)0x811c9dc5) // init value for FNV1 hash algorithm +#define FNV1_32_PRIME ((uint32_t)0x01000193) // magic prime for FNV1 hash algorithm + + /**** Trace macros, disable for production builds */ #define UBX_TRACE_PARSER(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* decoding progress in parse_char() */ #define UBX_TRACE_RXMSG(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* Rx msgs in payload_rx_done() */ @@ -90,11 +94,10 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _got_posllh(false), _got_velned(false), _disable_cmd_last(0), - _ack_waiting_msg(0) + _ack_waiting_msg(0), + _ubx_version(0) { decode_init(); - memset(_ubx_sw_version, 0, sizeof(_ubx_sw_version)); - memset(_ubx_hw_version, 0, sizeof(_ubx_hw_version)); } UBX::~UBX() @@ -633,18 +636,19 @@ UBX::payload_rx_add_mon_ver(const uint8_t b) _buf.raw[_rx_payload_index] = b; } else { if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) { - // Part 1 complete: decode Part 1 buffer - strncpy(_ubx_sw_version, (char*)_buf.payload_rx_mon_ver_part1.swVersion, sizeof(_ubx_sw_version)); - strncpy(_ubx_hw_version, (char*)_buf.payload_rx_mon_ver_part1.hwVersion, sizeof(_ubx_hw_version)); - UBX_WARN("VER sw %30s", _ubx_sw_version); - UBX_WARN("VER hw %10s", _ubx_hw_version); + // Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings + _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT); + _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version); + UBX_WARN("VER hash 0x%08x", _ubx_version); + UBX_WARN("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion); + UBX_WARN("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion); } // fill Part 2 buffer unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t); _buf.raw[buf_index] = b; if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) { // Part 2 complete: decode Part 2 buffer - UBX_WARN("VER ext %30s", _buf.payload_rx_mon_ver_part2.extension); + UBX_WARN("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension); } } @@ -877,3 +881,29 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len write(_fd, (const void *)payload, length); write(_fd, (const void *)&checksum, sizeof(checksum)); } + +uint32_t +UBX::fnv1_32_str(uint8_t *str, uint32_t hval) +{ + uint8_t *s = str; + + /* + * FNV-1 hash each octet in the buffer + */ + while (*s) { + + /* multiply by the 32 bit FNV magic prime mod 2^32 */ +#if defined(NO_FNV_GCC_OPTIMIZATION) + hval *= FNV1_32_PRIME; +#else + hval += (hval<<1) + (hval<<4) + (hval<<7) + (hval<<8) + (hval<<24); +#endif + + /* xor the bottom with the current octet */ + hval ^= (uint32_t)*s++; + } + + /* return our new hash value */ + return hval; +} + diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index c8c89dfad..f01b697a3 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -452,6 +452,11 @@ private: */ int wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report); + /** + * Calculate FNV1 hash + */ + uint32_t fnv1_32_str(uint8_t *str, uint32_t hval); + int _fd; struct vehicle_gps_position_s *_gps_position; struct satellite_info_s *_satellite_info; @@ -470,8 +475,7 @@ private: hrt_abstime _disable_cmd_last; uint16_t _ack_waiting_msg; ubx_buf_t _buf; - char _ubx_sw_version[30]; - char _ubx_hw_version[10]; + uint32_t _ubx_version; }; #endif /* UBX_H_ */ -- cgit v1.2.3 From 91f0b9eee41a8446c0a5ec455fbe3853c5c3eee3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 16 Jun 2014 17:32:58 +0200 Subject: mavlink: store last heartbeat time in telemetry_status topic --- src/modules/mavlink/mavlink_receiver.cpp | 18 ++++++++++++++++++ src/modules/uORB/topics/telemetry_status.h | 1 + 2 files changed, 19 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 33358b7b6..32c5e51dd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,6 +106,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _telemetry_heartbeat_time(0), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), @@ -150,6 +151,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_manual_control(msg); break; + case MAVLINK_MSG_ID_HEARTBEAT: + handle_message_heartbeat(msg); + break; + default: break; } @@ -411,6 +416,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) memset(&tstatus, 0, sizeof(tstatus)); tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = _telemetry_heartbeat_time; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -451,6 +457,18 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) +{ + mavlink_heartbeat_t hb; + mavlink_msg_heartbeat_decode(msg, &hb); + + /* ignore own heartbeats, accept only heartbeats from GCS */ + if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { + _telemetry_heartbeat_time = hrt_absolute_time(); + } +} + void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 76693c46e..e9e00d76c 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -57,6 +57,7 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; + uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ uint8_t rssi; /**< local signal strength */ uint8_t remote_rssi; /**< remote signal strength */ -- cgit v1.2.3 From e0ed0625f81841194b4bd9b26c7e047a1f68a1fc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 16 Jun 2014 17:34:21 +0200 Subject: commander: failsafe_state removed, replaced with bool failsafe, navigation state and failsafe determined directly from main state and conditions --- src/modules/commander/commander.cpp | 90 +++++++++----------- src/modules/commander/state_machine_helper.cpp | 112 +++++++++++++++---------- src/modules/commander/state_machine_helper.h | 2 - src/modules/gpio_led/gpio_led.c | 2 +- src/modules/mavlink/mavlink_messages.cpp | 4 +- src/modules/mavlink/mavlink_receiver.h | 2 + src/modules/navigator/navigator_main.cpp | 4 +- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/uORB/topics/vehicle_status.h | 25 +++--- 9 files changed, 125 insertions(+), 118 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e992706ac..43683f833 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include #include @@ -122,6 +123,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 +#define DL_TIMEOUT 5 * 1000* 1000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -495,12 +497,12 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -639,13 +641,6 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[5] = "REBOOT"; arming_states_str[6] = "IN_AIR_RESTORE"; - char *failsafe_states_str[FAILSAFE_STATE_MAX]; - failsafe_states_str[0] = "NORMAL"; - failsafe_states_str[1] = "RTL_RC"; - failsafe_states_str[2] = "RTL_DL"; - failsafe_states_str[3] = "LAND"; - failsafe_states_str[4] = "TERMINATION"; - /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -666,10 +661,10 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAVIGATION_STATE_MANUAL; + status.nav_state = NAVIGATION_STATE_MANUAL; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; - status.failsafe_state = FAILSAFE_STATE_NORMAL; + status.failsafe = false; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -678,6 +673,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark all signals lost as long as they haven't been found */ status.rc_signal_lost = true; status.offboard_control_signal_lost = true; + status.data_link_lost = true; /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; @@ -771,6 +767,11 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); + /* Subscribe to telemetry status */ + int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + struct telemetry_status_s telemetry; + memset(&telemetry, 0, sizeof(telemetry)); + /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; @@ -908,6 +909,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } + orb_check(telemetry_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry); + } + orb_check(sensor_sub, &updated); if (updated) { @@ -1186,7 +1193,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); } - /* start RC input check */ + /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { @@ -1198,9 +1205,6 @@ int commander_thread_main(int argc, char *argv[]) if (status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; - - status.failsafe_state = FAILSAFE_STATE_NORMAL; - failsafe_state_changed = true; } } @@ -1271,12 +1275,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } - - // if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - // /* recover from failsafe */ - // (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - // } - /* evaluate the main state machine according to mode switches */ transition_result_t main_res = set_main_state_rc(&status, &sp_man); @@ -1295,34 +1293,24 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { - - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RC_LOSS; - } else { - status.failsafe_state = FAILSAFE_STATE_LAND; - } - failsafe_state_changed = true; - } else { - mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission"); - } } } - /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ - if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RC_LOSS; - mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); - } else { - /* this probably doesn't make sense since we are in mission and have global position */ - status.failsafe_state = FAILSAFE_STATE_LAND; + /* data link check */ + if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: data link regained"); + status.data_link_lost = false; + status_changed = true; + } + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + status.data_link_lost = false; + status_changed = true; } - failsafe_state_changed = true; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1377,7 +1365,7 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; - /* now set nav state according to failsafe and main state */ + /* now set navigation state according to failsafe and main state */ set_nav_state(&status); if (main_state_changed) { @@ -1388,7 +1376,7 @@ int commander_thread_main(int argc, char *argv[]) if (failsafe_state_changed) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); failsafe_state_changed = false; } @@ -1415,7 +1403,7 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); @@ -1519,7 +1507,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ @@ -1667,7 +1655,7 @@ set_control_mode() control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; - switch (status.set_nav_state) { + switch (status.nav_state) { case NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -1719,8 +1707,6 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c52e618ef..df718ff99 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -354,7 +354,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, } } - if (ret = TRANSITION_CHANGED) { + if (ret == TRANSITION_CHANGED) { current_status->hil_state = new_state; current_status->timestamp = hrt_absolute_time(); // XXX also set lockdown here @@ -363,67 +363,95 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, return ret; } - /** * Check failsafe and main status and set navigation status for navigator accordingly */ void set_nav_state(struct vehicle_status_s *status) { - switch (status->failsafe_state) { - case FAILSAFE_STATE_NORMAL: - /* evaluate main state to decide in normal (non-failsafe) mode */ - switch (status->main_state) { - case MAIN_STATE_MANUAL: - status->set_nav_state = NAVIGATION_STATE_MANUAL; - break; - - case MAIN_STATE_ALTCTL: - status->set_nav_state = NAVIGATION_STATE_ALTCTL; - break; + status->failsafe = false; - case MAIN_STATE_POSCTL: - status->set_nav_state = NAVIGATION_STATE_POSCTL; - break; - - case MAIN_STATE_AUTO_MISSION: - status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; - break; - - case MAIN_STATE_AUTO_LOITER: - status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; - break; + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case MAIN_STATE_ACRO: + case MAIN_STATE_MANUAL: + case MAIN_STATE_ALTCTL: + case MAIN_STATE_POSCTL: + /* require RC for all manual modes */ + if (status->rc_signal_lost) { + status->failsafe = true; - case MAIN_STATE_AUTO_RTL: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL; - break; + } else { + switch (status->main_state) { + case MAIN_STATE_ACRO: + status->nav_state = NAVIGATION_STATE_ACRO; + break; + + case MAIN_STATE_MANUAL: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + + case MAIN_STATE_ALTCTL: + status->nav_state = NAVIGATION_STATE_ALTCTL; + break; + + case MAIN_STATE_POSCTL: + status->nav_state = NAVIGATION_STATE_POSCTL; + break; + + default: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + } + } + break; - case MAIN_STATE_ACRO: - status->set_nav_state = NAVIGATION_STATE_ACRO; - break; + case MAIN_STATE_AUTO_MISSION: + /* require data link and global position */ + if (status->data_link_lost || !status->condition_global_position_valid) { + status->failsafe = true; - default: - break; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; - case FAILSAFE_STATE_RC_LOSS: - status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS; - break; + case MAIN_STATE_AUTO_LOITER: + /* require data link and local position */ + if (status->data_link_lost || !status->condition_local_position_valid) { + status->failsafe = true; - case FAILSAFE_STATE_DL_LOSS: - status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } break; - case FAILSAFE_STATE_LAND: - status->set_nav_state = NAVIGATION_STATE_LAND; - break; + case MAIN_STATE_AUTO_RTL: + /* require global position and home */ + if (!status->condition_global_position_valid || !status->condition_home_position_valid) { + status->failsafe = true; - case FAILSAFE_STATE_TERMINATION: - status->set_nav_state = NAVIGATION_STATE_TERMINATION; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + } break; default: break; } + + if (status->failsafe) { + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 594bf23a1..062271764 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -63,8 +63,6 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); - transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); void set_nav_state(struct vehicle_status_s *status); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 6dfd22fdf..839a7890b 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -264,7 +264,7 @@ void gpio_led_cycle(FAR void *arg) pattern = 0x2A; // *_*_*_ fast blink (armed, error) } else if (priv->status.arming_state == ARMING_STATE_ARMED) { - if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) { pattern = 0x3f; // ****** solid (armed) } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b58fb4cb8..d7f56b63d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -118,7 +118,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - switch (status->set_nav_state) { + switch (status->nav_state) { case NAVIGATION_STATE_MANUAL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED @@ -161,8 +161,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..ab3dc81c6 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,6 +112,7 @@ private: void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); @@ -138,6 +139,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + hrt_abstime _telemetry_heartbeat_time; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a3c190c7f..70da5393f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -336,7 +336,7 @@ Navigator::task_main() } /* Do stuff according to navigation state set by commander */ - switch (_vstatus.set_nav_state) { + switch (_vstatus.nav_state) { case NAVIGATION_STATE_MANUAL: case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: @@ -351,8 +351,6 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: _navigation_mode = &_rtl; break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 53cd6a797..19872bbd0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1117,7 +1117,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state; + log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9390ff717..898c176fb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -59,7 +59,9 @@ * @addtogroup topics @{ */ -/* main state machine */ +/** + * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. + */ typedef enum { MAIN_STATE_MANUAL = 0, MAIN_STATE_ALTCTL, @@ -89,15 +91,9 @@ typedef enum { HIL_STATE_ON } hil_state_t; -typedef enum { - FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RC_LOSS, /**< Return To Launch on remote control loss */ - FAILSAFE_STATE_DL_LOSS, /**< Return To Launch on datalink loss */ - FAILSAFE_STATE_LAND, /**< Land as safe as possible */ - FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ - FAILSAFE_STATE_MAX, -} failsafe_state_t; - +/** + * Navigation state, i.e. "what should vehicle do". + */ typedef enum { NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ NAVIGATION_STATE_ACRO, /**< Acro mode */ @@ -106,9 +102,8 @@ typedef enum { NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ - NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS, /**< Auto failsafe on RC loss mode */ - NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS, /**< Auto failsafe on datalink loss mode */ NAVIGATION_STATE_LAND, /**< Land mode */ + NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ } navigation_state_t; @@ -171,10 +166,10 @@ struct vehicle_status_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ + navigation_state_t nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ - failsafe_state_t failsafe_state; /**< current failsafe state */ + bool failsafe; /**< true if system is in failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -199,6 +194,8 @@ struct vehicle_status_s { bool rc_signal_lost; /**< true if RC reception lost */ bool rc_input_blocked; /**< set if RC input should be ignored */ + bool data_link_lost; /**< datalink to GCS lost */ + bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; -- cgit v1.2.3 From 61833905472465ebab615ecdd567b78eb1628d97 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Mon, 16 Jun 2014 17:47:10 -0400 Subject: Commander attitude flag fix --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7baaf7139..3da43a792 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1835,7 +1835,7 @@ set_control_mode() break; case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = true; -- cgit v1.2.3 From 55e5f747de013fb386727b41cc67bd019c129c31 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 17 Jun 2014 13:19:50 +0200 Subject: commander: modes display fixes, don't activate failsafe while disarmed --- src/modules/commander/commander.cpp | 56 +++++++++++++++++--------- src/modules/commander/state_machine_helper.cpp | 32 +++++++++++---- src/modules/commander/state_machine_helper.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 3 +- 4 files changed, 66 insertions(+), 27 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 43683f833..f8dcec8cb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -624,22 +624,34 @@ int commander_thread_main(int argc, char *argv[]) warnx("starting"); char *main_states_str[MAIN_STATE_MAX]; - main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTCTL"; - main_states_str[2] = "POSCTL"; - main_states_str[3] = "AUTO_MISSION"; - main_states_str[4] = "AUTO_LOITER"; - main_states_str[5] = "AUTO_RTL"; - main_states_str[6] = "ACRO"; + main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; + main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; + main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; + main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[MAIN_STATE_ACRO] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; - arming_states_str[0] = "INIT"; - arming_states_str[1] = "STANDBY"; - arming_states_str[2] = "ARMED"; - arming_states_str[3] = "ARMED_ERROR"; - arming_states_str[4] = "STANDBY_ERROR"; - arming_states_str[5] = "REBOOT"; - arming_states_str[6] = "IN_AIR_RESTORE"; + arming_states_str[ARMING_STATE_INIT] = "INIT"; + arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; + arming_states_str[ARMING_STATE_ARMED] = "ARMED"; + arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; + arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; + arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + + char *nav_states_str[NAVIGATION_STATE_MAX]; + nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; + nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; + nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; + nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -844,7 +856,7 @@ int commander_thread_main(int argc, char *argv[]) /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; - bool failsafe_state_changed = false; + bool failsafe_old = false; while (!thread_should_exit) { @@ -1366,18 +1378,26 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; /* now set navigation state according to failsafe and main state */ - set_nav_state(&status); + bool nav_state_changed = set_nav_state(&status); + // TODO handle mode changes by commands if (main_state_changed) { status_changed = true; + warnx("main state: %s", main_states_str[status.main_state]); mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); main_state_changed = false; } - if (failsafe_state_changed) { + if (status.failsafe != failsafe_old) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); - failsafe_state_changed = false; + failsafe_old = status.failsafe; + } + + if (nav_state_changed) { + status_changed = true; + warnx("nav state: %s", nav_states_str[status.nav_state]); + mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index df718ff99..c0d730949 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -366,8 +366,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, /** * Check failsafe and main status and set navigation status for navigator accordingly */ -void set_nav_state(struct vehicle_status_s *status) +bool set_nav_state(struct vehicle_status_s *status) { + navigation_state_t nav_state_old = status->nav_state; + + bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR); status->failsafe = false; /* evaluate main state to decide in normal (non-failsafe) mode */ @@ -377,7 +380,7 @@ void set_nav_state(struct vehicle_status_s *status) case MAIN_STATE_ALTCTL: case MAIN_STATE_POSCTL: /* require RC for all manual modes */ - if (status->rc_signal_lost) { + if (status->rc_signal_lost && armed) { status->failsafe = true; } else { @@ -407,31 +410,44 @@ void set_nav_state(struct vehicle_status_s *status) case MAIN_STATE_AUTO_MISSION: /* require data link and global position */ - if (status->data_link_lost || !status->condition_global_position_valid) { + if ((status->data_link_lost || !status->condition_global_position_valid) && armed) { status->failsafe = true; } else { - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + if (armed) { + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + + } else { + // TODO which mode should we set when disarmed? + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } } break; case MAIN_STATE_AUTO_LOITER: /* require data link and local position */ - if (status->data_link_lost || !status->condition_local_position_valid) { + if ((status->data_link_lost || !status->condition_local_position_valid) && armed) { status->failsafe = true; } else { + // TODO which mode should we set when disarmed? status->nav_state = NAVIGATION_STATE_AUTO_LOITER; } break; case MAIN_STATE_AUTO_RTL: /* require global position and home */ - if (!status->condition_global_position_valid || !status->condition_home_position_valid) { + if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) { status->failsafe = true; } else { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + if (armed) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + + } else { + // TODO which mode should we set when disarmed? + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } } break; @@ -453,5 +469,7 @@ void set_nav_state(struct vehicle_status_s *status) status->nav_state = NAVIGATION_STATE_TERMINATION; } } + + return status->nav_state != nav_state_old; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 062271764..cffbc9b8f 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -void set_nav_state(struct vehicle_status_s *status); +bool set_nav_state(struct vehicle_status_s *status); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 898c176fb..851938795 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -96,15 +96,16 @@ typedef enum { */ typedef enum { NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ - NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */ NAVIGATION_STATE_POSCTL, /**< Position control mode */ NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ + NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ + NAVIGATION_STATE_MAX, } navigation_state_t; enum VEHICLE_MODE_FLAG { -- cgit v1.2.3 From e078ef992fc21d86cbd09db89c25332273840b22 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Tue, 17 Jun 2014 13:21:20 -0400 Subject: Removed publications closing This is an attempt to correct the offboard setpoints being passed on as "NaN" values --- src/modules/mavlink/mavlink_receiver.cpp | 59 ---------------------- src/modules/mc_att_control/mc_att_control_main.cpp | 8 +-- src/modules/mc_pos_control/mc_pos_control_main.cpp | 20 +------- 3 files changed, 3 insertions(+), 84 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c95ee880c..5528aca5e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -440,22 +440,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message loc_pos_sp.yaw = offboard_control_sp.p3; loc_pos_sp.z = -offboard_control_sp.p4; - /* Close fds to allow position controller to use attitude controller */ - if (_att_sp_pub > 0) { - close(_att_sp_pub); - _att_sp_pub = -1; - } - - if (_rates_sp_pub > 0) { - close(_rates_sp_pub); - _rates_sp_pub = -1; - } - - if (_global_vel_sp_pub > 0) { - close(_global_vel_sp_pub); - _global_vel_sp_pub = -1; - } - if (_local_pos_sp_pub < 0) { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub); @@ -472,16 +456,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message global_vel_sp.vy = offboard_control_sp.p2; global_vel_sp.vz = offboard_control_sp.p3; - if (_att_sp_pub > 0) { - close(_att_sp_pub); - _att_sp_pub = -1; - } - - if (_rates_sp_pub > 0) { - close(_rates_sp_pub); - _rates_sp_pub = -1; - } - if (_global_vel_sp_pub < 0) { _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp_pub); @@ -501,12 +475,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message att_sp.timestamp = hrt_absolute_time(); - /* Close fd to allow attitude controller to publish its own rates sp*/ - if (_rates_sp_pub > 0) { - close(_rates_sp_pub); - _rates_sp_pub = -1; - } - if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); @@ -1056,33 +1024,6 @@ MavlinkReceiver::receive_thread(void *arg) } } } - /* Close unused fds when not in offboard mode anymore */ - bool updated; - orb_check(_control_mode_sub, &updated); - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); - if (!_control_mode.flag_control_offboard_enabled) { - if (_local_pos_sp_pub > 0) { - close(_local_pos_sp_pub); - _local_pos_sp_pub = -1; - } - - if (_global_vel_sp_pub > 0) { - close(_global_vel_sp_pub); - _global_vel_sp_pub = -1; - } - - if (_att_sp_pub > 0) { - close(_att_sp_pub); - _att_sp_pub = -1; - } - - if (_rates_sp_pub > 0) { - close(_rates_sp_pub); - _rates_sp_pub = -1; - } - } - } } return NULL; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 26c0c386f..20e016da3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -584,15 +584,9 @@ MulticopterAttitudeControl::control_attitude(float dt) if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); - } else if (!_v_control_mode.flag_control_offboard_enabled) { + } else { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); } - - /* Close fd to let offboard att sp be advertised in mavlink receiver*/ - if (_v_control_mode.flag_control_offboard_enabled && _att_sp_pub > 0) { - close(_att_sp_pub); - _att_sp_pub = -1; - } } /* rotation matrix for current state */ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 35db05499..41fb20108 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -733,15 +733,10 @@ MulticopterPositionControl::task_main() if (_local_pos_sp_pub > 0) { orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); - } else if (!_control_mode.flag_control_offboard_enabled) { + } else { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); } - /* Close fd to let offboard pos sp be advertised in mavlink receiver*/ - if (_control_mode.flag_control_offboard_enabled && _local_pos_sp_pub > 0) { - close(_local_pos_sp_pub); - _local_pos_sp_pub = -1; - } if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ @@ -822,16 +817,10 @@ MulticopterPositionControl::task_main() if (_global_vel_sp_pub > 0) { orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); - } else if (!_control_mode.flag_control_offboard_enabled) { + } else { _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); } - /* Close fd to let offboard vel sp be advertised in mavlink receiver */ - if (_control_mode.flag_control_offboard_enabled && _global_vel_sp_pub > 0) { - close(_global_vel_sp_pub); - _global_vel_sp_pub = -1; - } - if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { /* reset integrals if needed */ if (_control_mode.flag_control_climb_rate_enabled) { @@ -1099,11 +1088,6 @@ MulticopterPositionControl::task_main() reset_int_z = true; reset_int_xy = true; - /* Close att_sp pub to allow offboard mode or att controller to advertise */ - if (_att_sp_pub > 0) { - close(_att_sp_pub); - _att_sp_pub = -1; - } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ -- cgit v1.2.3 From 0f41d8b5077856cf8d00f30acf9f4bc52f6e41a6 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 18 Jun 2014 13:43:03 +0800 Subject: Added pitch, roll, and yaw offsets to compensate for imperfect fmu placement. These were removed in 61a3177979838649af2a6e8e50bea7d15e2765f4 --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 66a949af1..95685b407 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -512,10 +512,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* send out */ att.timestamp = raw.timestamp; - - att.roll = euler[0]; - att.pitch = euler[1]; - att.yaw = euler[2] + ekf_params.mag_decl; + + /* Compensate for fmu placement offsets and magnetic declination */ + att.roll = euler[0] - ekf_params.roll_off; + att.pitch = euler[1] - ekf_params.pitch_off; + att.yaw = euler[2] + ekf_params.mag_decl - ekf_params.yaw_off; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; -- cgit v1.2.3 From 90a89ff2cdbb5268f4b8bee64b8da24db508f5a7 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 18 Jun 2014 20:27:35 +0800 Subject: WIP: Support in-flight fine tuning of board alignment. Implemented by applying a user supplied rotation matrix via new SENS_BOARD_(PITCH, ROLL, YAW)_OFF params in the sensors app. Currently only tested in QGC. --- src/modules/sensors/sensor_params.c | 24 ++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 26 +++++++++++++++++++++++--- 2 files changed, 47 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 999cf8bb3..c90633822 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -242,6 +242,30 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); */ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); +/** + * Board rotation pitch offset + * + * This parameter defines a pitch offset from the board rotation. It allows the user + * to fine tune the board offset in the event of misalignment. + */ + PARAM_DEFINE_FLOAT(SENS_BOARD_PITCH_OFF, 0); + +/** + * Board rotation roll offset + * + * This parameter defines a pitch offset from the board rotation. It allows the user + * to fine tune the board offset in the event of misalignment. + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_ROLL_OFF, 0); + +/** + * Board rotation YAW offset + * + * This parameter defines a pitch offset from the board rotation. It allows the user + * to fine tune the board offset in the event of misalignment. + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_YAW_OFF, 0); + /** * External magnetometer rotation * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b268b1b36..1a6202d7d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -230,6 +230,8 @@ private: math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ + math::Matrix<3, 3> _board_rotation_offset; /**< rotation matrix for fine tuning board offset **/ + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -252,6 +254,8 @@ private: int board_rotation; int external_mag_rotation; + + float board_offset[3]; int rc_map_roll; int rc_map_pitch; @@ -341,6 +345,8 @@ private: param_t board_rotation; param_t external_mag_rotation; + + param_t board_offset[3]; } _parameter_handles; /**< handles for interesting parameters */ @@ -587,6 +593,11 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + + /* rotation offsets */ + _parameter_handles.board_offset[0] = param_find("SENS_BOARD_ROLL_OFF"); + _parameter_handles.board_offset[1] = param_find("SENS_BOARD_PITCH_OFF"); + _parameter_handles.board_offset[2] = param_find("SENS_BOARD_YAW_OFF"); /* fetch initial parameter values */ parameters_update(); @@ -791,6 +802,15 @@ Sensors::parameters_update() get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); + param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); + param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); + + _board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); + return OK; } @@ -970,7 +990,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; + vect = _board_rotation * _board_rotation_offset * vect; raw.accelerometer_m_s2[0] = vect(0); raw.accelerometer_m_s2[1] = vect(1); @@ -996,7 +1016,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; + vect = _board_rotation * _board_rotation_offset * vect; raw.gyro_rad_s[0] = vect(0); raw.gyro_rad_s[1] = vect(1); @@ -1027,7 +1047,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) vect = _external_mag_rotation * vect; } else { - vect = _board_rotation * vect; + vect = _board_rotation * _board_rotation_offset * vect; } raw.magnetometer_ga[0] = vect(0); -- cgit v1.2.3 From 95003c6e1061b61dce6ed86e6c68d662e39a222d Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 18 Jun 2014 20:49:48 +0800 Subject: Removed unused ATT_XXX_OFF3 paramters. Offset tuning now accomplished via the sensors app using new SENS_BOARD_XXX_OFF parameters. --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 9 ++++----- .../attitude_estimator_ekf/attitude_estimator_ekf_params.c | 13 ------------- .../attitude_estimator_ekf/attitude_estimator_ekf_params.h | 1 - 3 files changed, 4 insertions(+), 19 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 95685b407..66a949af1 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -512,11 +512,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* send out */ att.timestamp = raw.timestamp; - - /* Compensate for fmu placement offsets and magnetic declination */ - att.roll = euler[0] - ekf_params.roll_off; - att.pitch = euler[1] - ekf_params.pitch_off; - att.yaw = euler[2] + ekf_params.mag_decl - ekf_params.yaw_off; + + att.roll = euler[0]; + att.pitch = euler[1]; + att.yaw = euler[2] + ekf_params.mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 3ff3d9922..49a892609 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -61,11 +61,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); /* offset estimation - UNUSED */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); -/* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); - /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); @@ -85,10 +80,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->r2 = param_find("EKF_ATT_V4_R2"); h->r3 = param_find("EKF_ATT_V4_R3"); - h->roll_off = param_find("ATT_ROLL_OFF3"); - h->pitch_off = param_find("ATT_PITCH_OFF3"); - h->yaw_off = param_find("ATT_YAW_OFF3"); - h->mag_decl = param_find("ATT_MAG_DECL"); h->acc_comp = param_find("ATT_ACC_COMP"); @@ -109,10 +100,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r2, &(p->r[2])); param_get(h->r3, &(p->r[3])); - param_get(h->roll_off, &(p->roll_off)); - param_get(h->pitch_off, &(p->pitch_off)); - param_get(h->yaw_off, &(p->yaw_off)); - param_get(h->mag_decl, &(p->mag_decl)); p->mag_decl *= M_PI / 180.0f; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 74a141609..5985541ca 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -54,7 +54,6 @@ struct attitude_estimator_ekf_params { struct attitude_estimator_ekf_param_handles { param_t r0, r1, r2, r3; param_t q0, q1, q2, q3, q4; - param_t roll_off, pitch_off, yaw_off; param_t mag_decl; param_t acc_comp; }; -- cgit v1.2.3 From 5ed1cf7e8d6201f1f2e0115f4941ac551f61d628 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 18 Jun 2014 21:34:38 +0800 Subject: Fixed too-long param names. --- src/modules/sensors/sensor_params.c | 10 +++++----- src/modules/sensors/sensors.cpp | 6 +++--- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c90633822..687efc49b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -248,23 +248,23 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); * This parameter defines a pitch offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ - PARAM_DEFINE_FLOAT(SENS_BOARD_PITCH_OFF, 0); + PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0); /** * Board rotation roll offset * - * This parameter defines a pitch offset from the board rotation. It allows the user + * This parameter defines a roll offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ -PARAM_DEFINE_FLOAT(SENS_BOARD_ROLL_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0); /** * Board rotation YAW offset * - * This parameter defines a pitch offset from the board rotation. It allows the user + * This parameter defines a yaw offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ -PARAM_DEFINE_FLOAT(SENS_BOARD_YAW_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0); /** * External magnetometer rotation diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 1a6202d7d..fe741894b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -595,9 +595,9 @@ Sensors::Sensors() : _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); /* rotation offsets */ - _parameter_handles.board_offset[0] = param_find("SENS_BOARD_ROLL_OFF"); - _parameter_handles.board_offset[1] = param_find("SENS_BOARD_PITCH_OFF"); - _parameter_handles.board_offset[2] = param_find("SENS_BOARD_YAW_OFF"); + _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); + _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); + _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); /* fetch initial parameter values */ parameters_update(); -- cgit v1.2.3 From e24925c743330bc3c6c0a24ba913a9c5fab5e07d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 18 Jun 2014 19:01:41 +0200 Subject: commander: added some failsafe logic --- src/modules/commander/commander.cpp | 39 +++++--- src/modules/commander/commander_params.c | 13 ++- src/modules/commander/state_machine_helper.cpp | 132 ++++++++++++++++++------- src/modules/commander/state_machine_helper.h | 2 +- src/modules/navigator/navigator_main.cpp | 4 + src/modules/uORB/topics/vehicle_status.h | 2 + 6 files changed, 142 insertions(+), 50 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f8dcec8cb..5954dcbb1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -619,6 +619,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_component_id = param_find("MAV_COMP_ID"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); + param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); /* welcome user */ warnx("starting"); @@ -627,9 +628,9 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; - main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; - main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; - main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[MAIN_STATE_ACRO] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -637,21 +638,23 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; arming_states_str[ARMING_STATE_ARMED] = "ARMED"; arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; - arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; - arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; char *nav_states_str[NAVIGATION_STATE_MAX]; - nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; - nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; - nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; - nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; - nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS] = "AUTO_FS_RC_LOSS"; + nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS] = "AUTO_FS_DL_LOSS"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; - nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -853,6 +856,8 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t arming_ret; + int32_t datalink_loss_enabled = false; + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -907,6 +912,7 @@ int commander_thread_main(int argc, char *argv[]) /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); + param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } orb_check(sp_man_sub, &updated); @@ -1309,8 +1315,8 @@ int commander_thread_main(int argc, char *argv[]) } /* data link check */ - if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) { - /* handle the case where RC signal was regained */ + if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where data link was regained */ if (status.data_link_lost) { mavlink_log_critical(mavlink_fd, "#audio: data link regained"); status.data_link_lost = false; @@ -1320,7 +1326,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = false; + status.data_link_lost = true; status_changed = true; } } @@ -1378,7 +1384,8 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; /* now set navigation state according to failsafe and main state */ - bool nav_state_changed = set_nav_state(&status); + bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, + mission_result.mission_finished); // TODO handle mode changes by commands if (main_state_changed) { @@ -1727,6 +1734,8 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_FS_RC_LOSS: + case NAVIGATION_STATE_AUTO_FS_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 80ca68f21..4750f9d5c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -39,7 +39,7 @@ * * @author Lorenz Meier * @author Thomas Gubler - * @author Julian Oes + * @author Julian Oes */ #include @@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); + +/** + * Datalink loss mode enabled. + * + * Set to 1 to enable actions triggered when the datalink is lost. + * + * @group commander + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c0d730949..f0fe13921 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -366,7 +366,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, /** * Check failsafe and main status and set navigation status for navigator accordingly */ -bool set_nav_state(struct vehicle_status_s *status) +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished) { navigation_state_t nav_state_old = status->nav_state; @@ -383,6 +383,16 @@ bool set_nav_state(struct vehicle_status_s *status) if (status->rc_signal_lost && armed) { status->failsafe = true; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } else { switch (status->main_state) { case MAIN_STATE_ACRO: @@ -409,45 +419,116 @@ bool set_nav_state(struct vehicle_status_s *status) break; case MAIN_STATE_AUTO_MISSION: - /* require data link and global position */ - if ((status->data_link_lost || !status->condition_global_position_valid) && armed) { + /* go into failsafe + * - if either the datalink is enabled and lost as well as RC is lost + * - if there is no datalink and the mission is finished */ + if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || + (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; - } else { - if (armed) { - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { + status->failsafe = true; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; } else { - // TODO which mode should we set when disarmed? - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_TERMINATION; } + + /* don't bother if RC is lost and mission is not yet finished */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for missions */ + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + } else { + /* everything is perfect */ + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; case MAIN_STATE_AUTO_LOITER: - /* require data link and local position */ - if ((status->data_link_lost || !status->condition_local_position_valid) && armed) { + /* go into failsafe if datalink and RC is lost */ + if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* go into failsafe if RC is lost and datalink loss is not set up */ + } else if (status->rc_signal_lost && !data_link_loss_enabled) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* don't bother if RC is lost if datalink is connected */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for loitering */ + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; } else { - // TODO which mode should we set when disarmed? + /* everything is perfect */ status->nav_state = NAVIGATION_STATE_AUTO_LOITER; } break; case MAIN_STATE_AUTO_RTL: /* require global position and home */ - if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) { + if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { status->failsafe = true; - } else { - if (armed) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; - + if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; } else { - // TODO which mode should we set when disarmed? - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_TERMINATION; } + } else { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } break; @@ -455,21 +536,6 @@ bool set_nav_state(struct vehicle_status_s *status) break; } - if (status->failsafe) { - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; - - } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; - - } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; - - } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; - } - } - return status->nav_state != nav_state_old; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index cffbc9b8f..2e076cdae 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -bool set_nav_state(struct vehicle_status_s *status); +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 70da5393f..50bcfd58d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,8 +351,12 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_FS_RC_LOSS: _navigation_mode = &_rtl; break; + case NAVIGATION_STATE_AUTO_FS_DL_LOSS: + _navigation_mode = &_rtl; /* TODO: change this to something else */ + break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: default: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 851938795..16652fddf 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -101,6 +101,8 @@ typedef enum { NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ + NAVIGATION_STATE_AUTO_FS_RC_LOSS, /**< Auto failsafe mode on RC loss */ + NAVIGATION_STATE_AUTO_FS_DL_LOSS, /**< Auto failsafe mode on DL loss */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ -- cgit v1.2.3 From 9d18da4433773cfa02bec1d33fdb34e4d03d1442 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Wed, 18 Jun 2014 16:45:38 -0400 Subject: Adds NaN checks and setpoint resets for offboard posctl --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 31 ++++++++++++++-------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 41fb20108..a4c871d95 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -675,21 +675,30 @@ MulticopterPositionControl::task_main() orb_check(_local_pos_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, - &_local_pos_sp); + orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp); } - /* Make sure position control is selected i.e. not only velocity control */ - if (_control_mode.flag_control_position_enabled) { - _pos_sp(0) = _local_pos_sp.x; - _pos_sp(1) = _local_pos_sp.y; - } + if (isfinite(_local_pos_sp.x) && isfinite(_local_pos_sp.y) && isfinite(_local_pos_sp.z) && isfinite(_local_pos_sp.yaw)) { + /* If manual control overides offboard, cancel the offboard setpoint and stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; - if (_control_mode.flag_control_altitude_enabled) { - _pos_sp(2) = _local_pos_sp.z; - } + /* Make sure position control is selected i.e. not only velocity control */ + if (_control_mode.flag_control_position_enabled) { + _pos_sp(0) = _local_pos_sp.x; + _pos_sp(1) = _local_pos_sp.y; + } - _att_sp.yaw_body = _local_pos_sp.yaw; + if (_control_mode.flag_control_altitude_enabled) { + _pos_sp(2) = _local_pos_sp.z; + } + + _att_sp.yaw_body = _local_pos_sp.yaw; + + } else { + reset_pos_sp(); + reset_alt_sp(); + } } else { /* AUTO */ -- cgit v1.2.3 From 2475efe13d8f8d9d801276b73b98759676a64db6 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Thu, 19 Jun 2014 13:22:45 +0800 Subject: Pre-compute board orientation offsets on param update. --- src/modules/sensors/sensors.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index fe741894b..16fcb4c26 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -229,8 +229,6 @@ private: math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ - - math::Matrix<3, 3> _board_rotation_offset; /**< rotation matrix for fine tuning board offset **/ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -807,10 +805,13 @@ Sensors::parameters_update() param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); - _board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], + /** fine tune board offset on parameter update **/ + math::Matrix<3, 3> board_rotation_offset; + board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], M_DEG_TO_RAD_F * _parameters.board_offset[1], M_DEG_TO_RAD_F * _parameters.board_offset[2]); - + + _board_rotation = _board_rotation * board_rotation_offset; return OK; } @@ -990,7 +991,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * _board_rotation_offset * vect; + vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); raw.accelerometer_m_s2[1] = vect(1); @@ -1016,7 +1017,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * _board_rotation_offset * vect; + vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); raw.gyro_rad_s[1] = vect(1); @@ -1047,7 +1048,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) vect = _external_mag_rotation * vect; } else { - vect = _board_rotation * _board_rotation_offset * vect; + vect = _board_rotation * vect; } raw.magnetometer_ga[0] = vect(0); -- cgit v1.2.3 From 94e004955df3f467b7e67b3fac0d968b9a68e091 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 19 Jun 2014 09:33:28 +0200 Subject: mavlink: publish telemtry status without radio status --- src/modules/mavlink/mavlink_receiver.cpp | 22 ++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 23 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 32c5e51dd..7a6922bfa 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -107,6 +107,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _rc_pub(-1), _manual_pub(-1), _telemetry_heartbeat_time(0), + _radio_status_available(false), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), @@ -432,6 +433,9 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); } + + /* this means that heartbeats alone won't be published to the radio status no more */ + _radio_status_available = true; } void @@ -467,6 +471,24 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { _telemetry_heartbeat_time = hrt_absolute_time(); } + + /* if no radio status messages arrive, lets at least publish that heartbeats were received */ + if (!_radio_status_available) { + + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = _telemetry_heartbeat_time; + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + + } else { + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } + } } void diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ab3dc81c6..cd1dab365 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -140,6 +140,7 @@ private: orb_advert_t _rc_pub; orb_advert_t _manual_pub; hrt_abstime _telemetry_heartbeat_time; + bool _radio_status_available; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; -- cgit v1.2.3 From 62faa2ee5155253779d3b93c5a280e8918585f6e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 19 Jun 2014 10:26:56 +0200 Subject: commander/navigator: renamed FS modes to RTL and RTGS (return to ground station) --- src/modules/commander/commander.cpp | 6 ++---- src/modules/commander/px4_custom_mode.h | 1 + src/modules/commander/state_machine_helper.cpp | 12 ++++++------ src/modules/mavlink/mavlink_messages.cpp | 8 ++++++++ src/modules/navigator/navigator_main.cpp | 3 +-- src/modules/uORB/topics/vehicle_status.h | 5 ++--- 6 files changed, 20 insertions(+), 15 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5954dcbb1..b957ad9b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -649,8 +649,7 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; - nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS] = "AUTO_FS_RC_LOSS"; - nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS] = "AUTO_FS_DL_LOSS"; + nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; @@ -1734,8 +1733,7 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FS_RC_LOSS: - case NAVIGATION_STATE_AUTO_FS_DL_LOSS: + case NAVIGATION_STATE_AUTO_RTGS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e0f8dc95d..7f5f93801 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -25,6 +25,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_LAND, + PX4_CUSTOM_SUB_MODE_AUTO_RTGS }; union px4_custom_mode { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f0fe13921..74885abf6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -384,7 +384,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -427,7 +427,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -441,7 +441,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -467,7 +467,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -481,7 +481,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -495,7 +495,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d7f56b63d..1fd8b1a27 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -176,6 +176,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; break; + case NAVIGATION_STATE_AUTO_RTGS: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; + break; + case NAVIGATION_STATE_TERMINATION: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 50bcfd58d..661d3d744 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,10 +351,9 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FS_RC_LOSS: _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_FS_DL_LOSS: + case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 16652fddf..5dc46dacb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -100,9 +100,8 @@ typedef enum { NAVIGATION_STATE_POSCTL, /**< Position control mode */ NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ - NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ - NAVIGATION_STATE_AUTO_FS_RC_LOSS, /**< Auto failsafe mode on RC loss */ - NAVIGATION_STATE_AUTO_FS_DL_LOSS, /**< Auto failsafe mode on DL loss */ + NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ + NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ -- cgit v1.2.3 From e0c78e51e3a5768014c73bed5cd087830d602227 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 19 Jun 2014 14:14:24 +0200 Subject: mavlink: only publish telemetry status from GCS --- src/modules/mavlink/mavlink_receiver.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7a6922bfa..baa6571a8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -470,23 +470,23 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) /* ignore own heartbeats, accept only heartbeats from GCS */ if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { _telemetry_heartbeat_time = hrt_absolute_time(); - } - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { + /* if no radio status messages arrive, lets at least publish that heartbeats were received */ + if (!_radio_status_available) { - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); - tstatus.timestamp = _telemetry_heartbeat_time; - tstatus.heartbeat_time = _telemetry_heartbeat_time; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + tstatus.timestamp = _telemetry_heartbeat_time; + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } else { + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } } } } -- cgit v1.2.3 From 45d5e2600911682a9117b26603e3213c6b918fa4 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Fri, 20 Jun 2014 14:37:44 +0800 Subject: Define float params properly: 0.0f instead of just 0 --- src/modules/sensors/sensor_params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 687efc49b..850e5f4e2 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -248,7 +248,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); * This parameter defines a pitch offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ - PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0); + PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); /** * Board rotation roll offset @@ -256,7 +256,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); * This parameter defines a roll offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ -PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); /** * Board rotation YAW offset @@ -264,7 +264,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0); * This parameter defines a yaw offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ -PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); /** * External magnetometer rotation -- cgit v1.2.3 From da2f68a6a09395a8d02a5d39bd3e92d7d0d79911 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Jun 2014 14:25:29 +0200 Subject: mavlink: don't lock dataman when updating mission state --- src/modules/mavlink/mavlink_mission.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 9bb956281..b2288469c 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -128,14 +128,8 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int mission.current_seq = seq; /* update mission state in dataman */ - /* lock MISSION_STATE item */ - dm_lock(DM_KEY_MISSION_STATE); - int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); - /* unlock MISSION_STATE item */ - dm_unlock(DM_KEY_MISSION_STATE); - if (res == sizeof(mission_s)) { /* update active mission state */ _dataman_id = dataman_id; -- cgit v1.2.3 From 6bab694e457a5c90b6500a2dc1f45b98fc36307c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Jun 2014 14:38:57 +0200 Subject: estimator: Improve error reporting and status printing (less flash, better resolution), move check and reset logic to a position AFTER the filter init. Do not externally zero the filter on resets but let the reset logic handle this. --- .../ekf_att_pos_estimator_main.cpp | 379 ++++++++++----------- 1 file changed, 187 insertions(+), 192 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 26a448b6d..d8dbb1fe3 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -96,7 +96,6 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); -static uint64_t last_run = 0; static uint64_t IMUmsec = 0; static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; @@ -191,19 +190,21 @@ private: float _baro_ref; /**< barometer reference altitude */ float _baro_gps_offset; /**< offset between GPS and baro */ - perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _loop_perf; ///< loop performance counter perf_counter_t _perf_gyro; ///CheckAndBound(); + + const char* ekfname = "[ekf] "; + + switch (check) { + case 0: + /* all ok */ + break; + case 1: + { + const char* str = "NaN in states, resetting"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 2: + { + const char* str = "stale IMU data, resetting"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 3: + { + const char* str = "switching to dynamic state"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 4: + { + const char* str = "excessive gyro offsets"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 5: + { + const char* str = "diverged too far from GPS"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 6: + { + const char* str = "excessive covariances"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + + default: + { + const char* str = "unknown reset condition"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + } + } + + struct estimator_status_report rep; + memset(&rep, 0, sizeof(rep)); + + struct ekf_status_report ekf_report; + + // If non-zero, we got a filter reset + if (check > 0 && check != 3) { + + // Count the reset condition + perf_count(_perf_reset); + + _ekf->GetLastErrorState(&ekf_report); + + // set sensors to de-initialized state + _gyro_valid = false; + _accel_valid = false; + _mag_valid = false; + + _baro_init = false; + _gps_initialized = false; + _initialized = false; + _last_sensor_timestamp = hrt_absolute_time(); + _last_run = _last_sensor_timestamp; + + _ekf->dtIMU = 0.01f; + + } else if (_ekf_logging) { + _ekf->GetFilterState(&ekf_report); + } + + if (_ekf_logging || check) { + rep.timestamp = hrt_absolute_time(); + + rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0); + rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1); + rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2); + rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3); + rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4); + rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5); + rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6); + rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7); + + rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); + rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); + rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); + rep.health_flags |= ((!(check == 4)) << 3); + + rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); + rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); + rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); + + // Copy all states or at least all that we can fit + unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); + unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); + rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; + + for (unsigned i = 0; i < rep.n_states; i++) { + rep.states[i] = ekf_report.states[i]; + } + + if (_estimator_status_pub > 0) { + orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); + } else { + _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); + } + } + + return check; +} + void FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) { @@ -545,7 +688,7 @@ FixedwingEstimator::task_main() _filter_start_time = hrt_absolute_time(); if (!_ekf) { - errx(1, "failed allocating EKF filter - out of RAM!"); + errx(1, "OUT OF MEM!"); } /* @@ -617,7 +760,7 @@ FixedwingEstimator::task_main() /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { - warn("poll error %d, %d", pret, errno); + warn("POLL ERR %d, %d", pret, errno); continue; } @@ -643,8 +786,6 @@ FixedwingEstimator::task_main() bool accel_updated; bool mag_updated; - hrt_abstime last_sensor_timestamp; - perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ @@ -668,12 +809,12 @@ FixedwingEstimator::task_main() _baro_init = false; _gps_initialized = false; - last_sensor_timestamp = hrt_absolute_time(); - last_run = last_sensor_timestamp; + _last_sensor_timestamp = hrt_absolute_time(); + _last_run = _last_sensor_timestamp; _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; - _filter_start_time = last_sensor_timestamp; + _filter_start_time = _last_sensor_timestamp; /* now skip this loop and get data on the next one, which will also re-init the filter */ continue; @@ -691,15 +832,14 @@ FixedwingEstimator::task_main() orb_check(_accel_sub, &accel_updated); if (accel_updated) { - perf_count(_perf_accel); orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } - last_sensor_timestamp = _gyro.timestamp; + _last_sensor_timestamp = _gyro.timestamp; IMUmsec = _gyro.timestamp / 1e3f; - float deltaT = (_gyro.timestamp - last_run) / 1e6f; - last_run = _gyro.timestamp; + float deltaT = (_gyro.timestamp - _last_run) / 1e6f; + _last_run = _gyro.timestamp; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { @@ -759,17 +899,17 @@ FixedwingEstimator::task_main() // Copy gyro and accel - last_sensor_timestamp = _sensor_combined.timestamp; + _last_sensor_timestamp = _sensor_combined.timestamp; IMUmsec = _sensor_combined.timestamp / 1e3f; - float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; + float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } - last_run = _sensor_combined.timestamp; + _last_run = _sensor_combined.timestamp; // Always store data, independent of init status /* fill in last data set */ @@ -787,6 +927,7 @@ FixedwingEstimator::task_main() } _gyro_valid = true; + perf_count(_perf_gyro); } if (accel_updated) { @@ -907,6 +1048,8 @@ FixedwingEstimator::task_main() warnx("ALT REF INIT"); } + perf_count(_perf_baro); + newHgtData = true; } else { newHgtData = false; @@ -964,144 +1107,6 @@ FixedwingEstimator::task_main() continue; } - - /* - * CHECK IF THE INPUT DATA IS SANE - */ - int check = _ekf->CheckAndBound(); - - const char* ekfname = "[ekf] "; - - switch (check) { - case 0: - /* all ok */ - break; - case 1: - { - const char* str = "NaN in states, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 2: - { - const char* str = "stale IMU data, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 3: - { - const char* str = "switching to dynamic state"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 4: - { - const char* str = "excessive gyro offsets"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 5: - { - const char* str = "diverged too far from GPS"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 6: - { - const char* str = "excessive covariances"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - - default: - { - const char* str = "unknown reset condition"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - } - } - - // warn on fatal resets - if (check == 1) { - warnx("NUMERIC ERROR IN FILTER"); - } - - struct estimator_status_report rep; - memset(&rep, 0, sizeof(rep)); - - struct ekf_status_report ekf_report; - - // If non-zero, we got a filter reset - if (check > 0 && check != 3) { - - _ekf->GetLastErrorState(&ekf_report); - - /* set sensors to de-initialized state */ - _gyro_valid = false; - _accel_valid = false; - _mag_valid = false; - - _baro_init = false; - _gps_initialized = false; - _initialized = false; - last_sensor_timestamp = hrt_absolute_time(); - last_run = last_sensor_timestamp; - - _ekf->ZeroVariables(); - _ekf->statesInitialised = false; - _ekf->dtIMU = 0.01f; - - // Let the system re-initialize itself - continue; - - } else if (_ekf_logging) { - _ekf->GetFilterState(&ekf_report); - } - - if (_ekf_logging || check) { - rep.timestamp = hrt_absolute_time(); - - rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0); - rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1); - rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2); - rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3); - rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4); - rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5); - rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6); - rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7); - - rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); - rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); - rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); - rep.health_flags |= ((!(check == 4)) << 3); - - rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); - rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); - rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); - - // Copy all states or at least all that we can fit - unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); - unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); - rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; - - for (unsigned i = 0; i < rep.n_states; i++) { - rep.states[i] = ekf_report.states[i]; - } - - if (_estimator_status_pub > 0) { - orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); - } else { - _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); - } - } - - /** * PART TWO: EXECUTE THE FILTER * @@ -1176,6 +1181,13 @@ FixedwingEstimator::task_main() // We're apparently initialized in this case now + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } + // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); @@ -1301,7 +1313,7 @@ FixedwingEstimator::task_main() for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) _att.R[i][j] = R(i, j); - _att.timestamp = last_sensor_timestamp; + _att.timestamp = _last_sensor_timestamp; _att.q[0] = _ekf->states[0]; _att.q[1] = _ekf->states[1]; _att.q[2] = _ekf->states[2]; @@ -1309,7 +1321,7 @@ FixedwingEstimator::task_main() _att.q_valid = true; _att.R_valid = true; - _att.timestamp = last_sensor_timestamp; + _att.timestamp = _last_sensor_timestamp; _att.roll = euler(0); _att.pitch = euler(1); _att.yaw = euler(2); @@ -1333,7 +1345,7 @@ FixedwingEstimator::task_main() } if (_gps_initialized) { - _local_pos.timestamp = last_sensor_timestamp; + _local_pos.timestamp = _last_sensor_timestamp; _local_pos.x = _ekf->states[7]; _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly @@ -1500,44 +1512,28 @@ FixedwingEstimator::print_status() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); + printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); + printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); + printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); + printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); + if (n_states == 23) { - printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); - printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); - printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); - printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); - printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); - printf("states: %s %s %s %s %s %s %s %s %s %s\n", - (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", - (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", - (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + } else { - printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); - printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); - printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); - printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); - printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); - printf("states: %s %s %s %s %s %s %s %s %s %s\n", + } + printf("states: %s %s %s %s %s %s %s %s %s %s\n", (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", @@ -1548,7 +1544,6 @@ FixedwingEstimator::print_status() (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); - } } int FixedwingEstimator::trip_nan() { -- cgit v1.2.3 From 0673740e0e0ed4bc2134805ee1d556b92d4b7da5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Jun 2014 14:58:37 +0200 Subject: Initialize velNED fields correctly, preventing a bogus initial filter reset --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index d364facad..673865dd4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -35,8 +35,11 @@ AttPosEKF::AttPosEKF() magDeclination = 0.0f; dAngIMU.zero(); dVelIMU.zero(); - ekfDiverged = false; + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; delAngTotal.zero(); + ekfDiverged = false; memset(&last_ekf_error, 0, sizeof(last_ekf_error)); ZeroVariables(); -- cgit v1.2.3 From f1a5be1f57d9a15f29a428e962c3ed7f75777d22 Mon Sep 17 00:00:00 2001 From: Kynos Date: Sat, 21 Jun 2014 20:42:33 +0200 Subject: Define NAV-PVT message --- src/drivers/gps/ubx.h | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index f01b697a3..1a9e7cb0c 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -63,6 +63,7 @@ /* Message IDs */ #define UBX_ID_NAV_POSLLH 0x02 #define UBX_ID_NAV_SOL 0x06 +#define UBX_ID_NAV_PVT 0x07 #define UBX_ID_NAV_VELNED 0x12 #define UBX_ID_NAV_TIMEUTC 0x21 #define UBX_ID_NAV_SVINFO 0x30 @@ -78,6 +79,7 @@ /* Message Classes & IDs */ #define UBX_MSG_NAV_POSLLH ((UBX_CLASS_NAV) | UBX_ID_NAV_POSLLH << 8) #define UBX_MSG_NAV_SOL ((UBX_CLASS_NAV) | UBX_ID_NAV_SOL << 8) +#define UBX_MSG_NAV_PVT ((UBX_CLASS_NAV) | UBX_ID_NAV_PVT << 8) #define UBX_MSG_NAV_VELNED ((UBX_CLASS_NAV) | UBX_ID_NAV_VELNED << 8) #define UBX_MSG_NAV_TIMEUTC ((UBX_CLASS_NAV) | UBX_ID_NAV_TIMEUTC << 8) #define UBX_MSG_NAV_SVINFO ((UBX_CLASS_NAV) | UBX_ID_NAV_SVINFO << 8) @@ -90,6 +92,18 @@ #define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) #define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8) +/* RX NAV-PVT message content details */ +/* Bitfield "valid" masks */ +#define UBX_RX_NAV_PVT_VALID_VALIDDATE 0x01 /**< validDate (Valid UTC Date) */ +#define UBX_RX_NAV_PVT_VALID_VALIDTIME 0x02 /**< validTime (Valid UTC Time) */ +#define UBX_RX_NAV_PVT_VALID_FULLYRESOLVED 0x04 /**< fullyResolved (1 = UTC Time of Day has been fully resolved (no seconds uncertainty)) */ + +/* Bitfield "flags" masks */ +#define UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK 0x01 /**< gnssFixOK (A valid fix (i.e within DOP & accuracy masks)) */ +#define UBX_RX_NAV_PVT_FLAGS_DIFFSOLN 0x02 /**< diffSoln (1 if differential corrections were applied) */ +#define UBX_RX_NAV_PVT_FLAGS_PSMSTATE 0x1C /**< psmState (Power Save Mode state (see Power Management)) */ +#define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID 0x20 /**< headVehValid (Heading of vehicle is valid) */ + /* TX CFG-PRT message contents */ #define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ #define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ @@ -162,6 +176,42 @@ typedef struct { uint32_t reserved2; } ubx_payload_rx_nav_sol_t; +/* Rx NAV-PVT */ +typedef struct { + uint32_t iTOW; /**< GPS Time of Week [ms] */ + uint16_t year; /**< Year (UTC)*/ + uint8_t month; /**< Month, range 1..12 (UTC) */ + uint8_t day; /**< Day of month, range 1..31 (UTC) */ + uint8_t hour; /**< Hour of day, range 0..23 (UTC) */ + uint8_t min; /**< Minute of hour, range 0..59 (UTC) */ + uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */ + uint8_t valid; /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */ + uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */ + int32_t nano; /**< Fraction of second (UTC) [-1e9...1e9 ns] */ + uint8_t fixType; /**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */ + uint8_t flags; /**< Fix Status Flags (see UBX_RX_NAV_PVT_FLAGS_...) */ + uint8_t reserved1; + uint8_t numSV; /**< Number of SVs used in Nav Solution */ + int32_t lon; /**< Longitude [1e-7 deg] */ + int32_t lat; /**< Latitude [1e-7 deg] */ + int32_t height; /**< Height above ellipsoid [mm] */ + int32_t hMSL; /**< Height above mean sea level [mm] */ + uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */ + uint32_t vAcc; /**< Vertical accuracy estimate [mm] */ + int32_t velN; /**< NED north velocity [mm/s]*/ + int32_t velE; /**< NED east velocity [mm/s]*/ + int32_t velD; /**< NED down velocity [mm/s]*/ + int32_t gSpeed; /**< Ground Speed (2-D) [mm/s] */ + int32_t headMot; /**< Heading of motion (2-D) [1e-5 deg] */ + uint32_t sAcc; /**< Speed accuracy estimate [mm/s] */ + uint32_t headAcc; /**< Heading accuracy estimate (motion and vehicle) [1e-5 deg] */ + uint16_t pDOP; /**< Position DOP [0.01] */ + uint16_t reserved2; + uint32_t reserved3; + int32_t headVeh; /**< Heading of vehicle (2-D) [1e-5 deg] */ + uint32_t reserved4; +} ubx_payload_rx_nav_pvt_t; + /* Rx NAV-TIMEUTC */ typedef struct { uint32_t iTOW; /**< GPS Time of Week [ms] */ -- cgit v1.2.3 From ebd68f4ffa2b6f3b6fdb1a19ef22b1aab54b4251 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Sun, 22 Jun 2014 16:43:05 +0800 Subject: Clarified parameter comments and added @group attributed for auto-generation. --- src/modules/sensors/sensor_params.c | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 850e5f4e2..c8a3ec8f0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -243,26 +243,32 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** - * Board rotation pitch offset + * Board rotation Y (Pitch) offset * - * This parameter defines a pitch offset from the board rotation. It allows the user + * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); /** - * Board rotation roll offset + * Board rotation X (Roll) offset * - * This parameter defines a roll offset from the board rotation. It allows the user + * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); /** - * Board rotation YAW offset + * Board rotation Z (YAW) offset * - * This parameter defines a yaw offset from the board rotation. It allows the user + * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); -- cgit v1.2.3 From 73d4d6faec818484750fc56b5129038abf9a5259 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Jun 2014 16:49:52 +0200 Subject: MPU6K: Use usleep where usleep should be used instead of up_udelay() --- src/drivers/mpu6000/mpu6000.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 321fdd173..0edec3d0e 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -544,7 +544,7 @@ void MPU6000::reset() write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); irqrestore(state); - up_udelay(1000); + usleep(1000); // SAMPLE RATE _set_sample_rate(_sample_rate); -- cgit v1.2.3 From 1018dec2468d2aa6f1b1961d8b7ae62911c821c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Jun 2014 16:51:02 +0200 Subject: estimator: Get offsets under control, prepare GPS acceleration based initialization, but do not activate it yet --- .../ekf_att_pos_estimator_main.cpp | 34 +++++++++++++++++----- 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index d8dbb1fe3..cb29b913c 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -188,7 +188,8 @@ private: struct map_projection_reference_s _pos_ref; float _baro_ref; /**< barometer reference altitude */ - float _baro_gps_offset; /**< offset between GPS and baro */ + float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ + float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ perf_counter_t _loop_perf; ///< loop performance counter perf_counter_t _perf_gyro; ///states[9]; // this should become zero in the local frame + _baro_gps_offset = _baro.altitude - gps_alt; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); @@ -1159,7 +1164,7 @@ FixedwingEstimator::task_main() #if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset); + warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); #endif @@ -1174,6 +1179,7 @@ FixedwingEstimator::task_main() _ekf->posNE[1] = posNED[1]; _local_pos.ref_alt = _baro_ref; + _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); @@ -1229,6 +1235,19 @@ FixedwingEstimator::task_main() // Fuse GPS Measurements if (newDataGps && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED + + float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f; + + // Calculate acceleration predicted by GPS velocity change + if ((_ekf->velNED[0] != _gps.vel_n_m_s || + _ekf->velNED[1] != _gps.vel_e_m_s || + _ekf->velNED[2] != _gps.vel_d_m_s) && (gps_dt > 0.00001f)) { + + _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; + _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; + _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; + } + _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; @@ -1349,7 +1368,7 @@ FixedwingEstimator::task_main() _local_pos.x = _ekf->states[7]; _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_gps_offset; + _local_pos.z = _ekf->states[9] - _baro_ref_offset; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; @@ -1411,7 +1430,7 @@ FixedwingEstimator::task_main() } /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z); + _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -1513,7 +1532,8 @@ FixedwingEstimator::print_status() // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); - printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); + printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); + printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); -- cgit v1.2.3 From ac319a724090d34c53f10fa867794f0c06dcd0ff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Jun 2014 16:51:48 +0200 Subject: 23 state estimator: Prepare GPS accel init (not yet active), clean up init calls --- .../ekf_att_pos_estimator/estimator_23states.cpp | 34 +++++++++++++--------- .../ekf_att_pos_estimator/estimator_23states.h | 2 ++ 2 files changed, 23 insertions(+), 13 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 673865dd4..9be48cfea 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -38,8 +38,12 @@ AttPosEKF::AttPosEKF() velNED[0] = 0.0f; velNED[1] = 0.0f; velNED[2] = 0.0f; + accelGPSNED[0] = 0.0f; + accelGPSNED[1] = 0.0f; + accelGPSNED[2] = 0.0f; delAngTotal.zero(); ekfDiverged = false; + lastReset = 0; memset(&last_ekf_error, 0, sizeof(last_ekf_error)); ZeroVariables(); @@ -2321,12 +2325,21 @@ int AttPosEKF::CheckAndBound() // Store the old filter state bool currStaticMode = staticMode; + // Limit reset rate to 5 Hz to allow the filter + // to settle + if (millis() - lastReset < 200) { + return 0; + } + if (ekfDiverged) { ekfDiverged = false; } int ret = 0; + // Check if we're on ground - this also sets static mode. + OnGroundCheck(); + // Reset the filter if the states went NaN if (StatesNaN(&last_ekf_error)) { ekf_debug("re-initializing dynamic"); @@ -2348,9 +2361,6 @@ int AttPosEKF::CheckAndBound() ret = 2; } - // Check if we're on ground - this also sets static mode. - OnGroundCheck(); - // Check if we switched between states if (currStaticMode != staticMode) { FillErrorReport(&last_ekf_error); @@ -2388,6 +2398,7 @@ int AttPosEKF::CheckAndBound() if (ret) { ekfDiverged = true; + lastReset = millis(); } return ret; @@ -2441,7 +2452,6 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { - ZeroVariables(); // Fill variables with valid data @@ -2478,17 +2488,13 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) magstate.R_MAG = sq(magMeasurementSigma); magstate.DCM = DCM; - // Calculate position from gps measurement - float posNEDInit[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m) - calcposNED(posNEDInit, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities // positions: - states[7] = posNEDInit[0]; - states[8] = posNEDInit[1]; - states[9] = posNEDInit[2]; + states[7] = posNE[0]; + states[8] = posNE[1]; + states[9] = -hgtMea; for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel states[16] = initMagNED.x; // Magnetic Field North states[17] = initMagNED.y; // Magnetic Field East @@ -2520,11 +2526,13 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do hgtRef = referenceHgt; refSet = true; - // we are at reference altitude, so measurement must be zero - hgtMea = 0.0f; + // we are at reference position, so measurement must be zero posNE[0] = 0.0f; posNE[1] = 0.0f; + // we are at an unknown, possibly non-zero altitude - so altitude + // is not reset (hgtMea) + // the baro offset must be this difference now baroHgtOffset = baroHgt - referenceHgt; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 1bf1312b0..10a646025 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -138,6 +138,7 @@ public: float varInnovVelPos[6]; // innovation variance output float velNED[3]; // North, East, Down velocity obs (m/s) + float accelGPSNED[3]; // Acceleration predicted by GPS in earth frame float posNE[2]; // North, East position obs (m) float hgtMea; // measured height (m) float baroHgtOffset; ///< the baro (weather) offset from normalized altitude @@ -185,6 +186,7 @@ public: bool useRangeFinder; ///< true when rangefinder is being used bool ekfDiverged; + uint64_t lastReset; struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; -- cgit v1.2.3 From f318ee21943aa6902a7d4d0c092880a4d3c4ce8c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Jun 2014 18:15:15 +0200 Subject: Remove debug output in commander spamming console --- src/modules/commander/commander.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b957ad9b5..df7a4c886 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1592,7 +1592,6 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin switch (sp_man->mode_switch) { case SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; - warnx("NONE"); break; case SWITCH_POS_OFF: // MANUAL -- cgit v1.2.3 From 72afa2ca2bb7ce85262dd201b7620e310484f6c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Jun 2014 18:15:40 +0200 Subject: Capture TX issues in performance counter instead of spamming console in mavlink app --- src/modules/mavlink/mavlink_main.cpp | 30 ++++++++++++++++++++++++------ src/modules/mavlink/mavlink_main.h | 6 ++++++ 2 files changed, 30 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d7cf45351..5f4236b3e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -192,13 +192,14 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (buf_free < desired) { /* we don't want to send anything just in half, so return */ + instance->count_txerr(); return; } } ssize_t ret = write(uart, ch, desired); if (ret != desired) { - warnx("TX FAIL"); + instance->count_txerr(); } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } @@ -237,7 +238,8 @@ Mavlink::Mavlink() : _flow_control_enabled(true), _message_buffer({}), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { _wpm = &_wpm_s; mission.count = 0; @@ -290,6 +292,7 @@ Mavlink::Mavlink() : Mavlink::~Mavlink() { perf_free(_loop_perf); + perf_free(_txerr_perf); if (_task_running) { /* task wakes up every 10ms or so at the longest */ @@ -314,6 +317,12 @@ Mavlink::~Mavlink() LL_DELETE(_mavlink_instances, this); } +void +Mavlink::count_txerr() +{ + perf_count(_txerr_perf); +} + void Mavlink::set_mode(enum MAVLINK_MODE mode) { @@ -2169,11 +2178,20 @@ int Mavlink::start_helper(int argc, char *argv[]) /* create the instance in task context */ Mavlink *instance = new Mavlink(); - /* this will actually only return once MAVLink exits */ - int res = instance->task_main(argc, argv); + int res; - /* delete instance on main thread end */ - delete instance; + if (!instance) { + + /* out of memory */ + res = -ENOMEM; + warnx("OUT OF MEM"); + } else { + /* this will actually only return once MAVLink exits */ + res = instance->task_main(argc, argv); + + /* delete instance on main thread end */ + delete instance; + } return res; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 40edc4b85..674efe01e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -213,6 +213,11 @@ public: bool get_wait_to_transmit() { return _wait_to_transmit; } bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + /** + * Count a transmision error + */ + void count_txerr(); + protected: Mavlink *next; @@ -282,6 +287,7 @@ private: pthread_mutex_t _message_buffer_mutex; perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ /** * Send one parameter. -- cgit v1.2.3 From 90a40dda866b8c8db7f183f7f184d5d4edaa7620 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Jun 2014 22:21:19 +0200 Subject: Compile fix --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index a194930a5..35dc39ec6 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -342,7 +342,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); - if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); /* update mag declination rotation matrix */ @@ -477,7 +477,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ - if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); } else { -- cgit v1.2.3 From 60e93deaa3971dccba7f1e3184a69f6bafe391d8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Jun 2014 07:44:30 +0200 Subject: phantom: silence ESC --- ROMFS/px4fmu_common/init.d/3031_phantom | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 24372bddc..d05c3174f 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -37,3 +37,7 @@ then fi set MIXER FMU_Q + +# Provide ESC a constant 1000 us pulse +set PWM_OUTPUTS 4 +set PWM_DISARMED 1000 -- cgit v1.2.3 From 9f3758582f31066ba6ec0af2efc5c959fabb1baf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Jun 2014 10:18:16 +0200 Subject: update mavlink headers, headers generated with message definitions from the externalsetpoints branch of mavlink --- .../mavlink/v1.0/ardupilotmega/ardupilotmega.h | 122 ++++- .../v1.0/ardupilotmega/mavlink_msg_camera_event.h | 401 ++++++++++++++++ .../ardupilotmega/mavlink_msg_camera_feedback.h | 473 +++++++++++++++++++ .../include/mavlink/v1.0/ardupilotmega/testsuite.h | 126 +++++ .../include/mavlink/v1.0/ardupilotmega/version.h | 2 +- mavlink/include/mavlink/v1.0/autoquad/autoquad.h | 21 +- mavlink/include/mavlink/v1.0/autoquad/version.h | 2 +- mavlink/include/mavlink/v1.0/checksum.h | 2 +- mavlink/include/mavlink/v1.0/common/common.h | 146 +++--- .../mavlink_msg_attitude_setpoint_external.h | 393 ++++++++++++++++ .../v1.0/common/mavlink_msg_distance_sensor.h | 78 +-- ...ink_msg_global_position_setpoint_external_int.h | 497 ++++++++++++++++++++ .../mavlink/v1.0/common/mavlink_msg_heartbeat.h | 10 +- ...link_msg_local_ned_position_setpoint_external.h | 521 +++++++++++++++++++++ mavlink/include/mavlink/v1.0/common/testsuite.h | 204 +++++++- mavlink/include/mavlink/v1.0/common/version.h | 2 +- .../include/mavlink/v1.0/matrixpilot/matrixpilot.h | 21 +- mavlink/include/mavlink/v1.0/matrixpilot/version.h | 2 +- mavlink/include/mavlink/v1.0/mavlink_helpers.h | 21 +- mavlink/include/mavlink/v1.0/mavlink_types.h | 4 +- mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h | 21 +- mavlink/include/mavlink/v1.0/pixhawk/version.h | 2 +- mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h | 14 +- mavlink/include/mavlink/v1.0/sensesoar/version.h | 2 +- 24 files changed, 2917 insertions(+), 170 deletions(-) create mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_camera_event.h create mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_camera_feedback.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_setpoint_external.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_external_int.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_local_ned_position_setpoint_external.h diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index c887dc68a..7e9173247 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -5,6 +5,10 @@ #ifndef ARDUPILOTMEGA_H #define ARDUPILOTMEGA_H +#ifndef MAVLINK_H + #error Wrong include order: ARDUPILOTMEGA.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. +#endif + #ifdef __cplusplus extern "C" { #endif @@ -12,15 +16,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 20, 24, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 39, 45, 44, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 20, 24, 29, 41, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 212, 121, 172, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 206, 151, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS, MAVLINK_MESSAGE_INFO_AHRS2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_ATTITUDE_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS, MAVLINK_MESSAGE_INFO_AHRS2, MAVLINK_MESSAGE_INFO_CAMERA_EVENT, MAVLINK_MESSAGE_INFO_CAMERA_FEEDBACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -30,10 +34,65 @@ extern "C" { // ENUM DEFINITIONS +/** @brief */ +#ifndef HAVE_ENUM_MAV_CMD +#define HAVE_ENUM_MAV_CMD +typedef enum MAV_CMD +{ + MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Custom mode - this is system specific, please refer to the individual autopilot specifications for details.| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOTOR_TEST=209, /* Mission command to perform motor test |motor sequence number (a number from 1 to max number of motors on the vehicle)| throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)| throttle| timeout (in seconds)| Empty| Empty| Empty| */ + MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ + MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ + MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ + MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ + MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ + MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ + MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ + MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ +} MAV_CMD; +#endif + /** @brief */ #ifndef HAVE_ENUM_LIMITS_STATE #define HAVE_ENUM_LIMITS_STATE -enum LIMITS_STATE +typedef enum LIMITS_STATE { LIMITS_INIT=0, /* pre-initialization | */ LIMITS_DISABLED=1, /* disabled | */ @@ -42,42 +101,81 @@ enum LIMITS_STATE LIMITS_RECOVERING=4, /* taking action eg. RTL | */ LIMITS_RECOVERED=5, /* we're no longer in breach of a limit | */ LIMITS_STATE_ENUM_END=6, /* | */ -}; +} LIMITS_STATE; #endif /** @brief */ #ifndef HAVE_ENUM_LIMIT_MODULE #define HAVE_ENUM_LIMIT_MODULE -enum LIMIT_MODULE +typedef enum LIMIT_MODULE { LIMIT_GPSLOCK=1, /* pre-initialization | */ LIMIT_GEOFENCE=2, /* disabled | */ LIMIT_ALTITUDE=4, /* checking limits | */ LIMIT_MODULE_ENUM_END=5, /* | */ -}; +} LIMIT_MODULE; #endif /** @brief Flags in RALLY_POINT message */ #ifndef HAVE_ENUM_RALLY_FLAGS #define HAVE_ENUM_RALLY_FLAGS -enum RALLY_FLAGS +typedef enum RALLY_FLAGS { FAVORABLE_WIND=1, /* Flag set when requiring favorable winds for landing. | */ LAND_IMMEDIATELY=2, /* Flag set when plane is to immediately descend to break altitude and land without GCS intervention. Flag not set when plane is to loiter at Rally point until commanded to land. | */ RALLY_FLAGS_ENUM_END=3, /* | */ -}; +} RALLY_FLAGS; #endif /** @brief */ #ifndef HAVE_ENUM_PARACHUTE_ACTION #define HAVE_ENUM_PARACHUTE_ACTION -enum PARACHUTE_ACTION +typedef enum PARACHUTE_ACTION { PARACHUTE_DISABLE=0, /* Disable parachute release | */ PARACHUTE_ENABLE=1, /* Enable parachute release | */ PARACHUTE_RELEASE=2, /* Release parachute | */ PARACHUTE_ACTION_ENUM_END=3, /* | */ -}; +} PARACHUTE_ACTION; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MOTOR_TEST_THROTTLE_TYPE +#define HAVE_ENUM_MOTOR_TEST_THROTTLE_TYPE +typedef enum MOTOR_TEST_THROTTLE_TYPE +{ + MOTOR_TEST_THROTTLE_PERCENT=0, /* throttle as a percentage from 0 ~ 100 | */ + MOTOR_TEST_THROTTLE_PWM=1, /* throttle as an absolute PWM value (normally in range of 1000~2000) | */ + MOTOR_TEST_THROTTLE_PILOT=2, /* throttle pass-through from pilot's transmitter | */ + MOTOR_TEST_THROTTLE_TYPE_ENUM_END=3, /* | */ +} MOTOR_TEST_THROTTLE_TYPE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_CAMERA_EVENT_TYPES +#define HAVE_ENUM_CAMERA_EVENT_TYPES +typedef enum CAMERA_EVENT_TYPES +{ + HEARTBEAT=0, /* Camera heartbeat, announce camera component ID at 1hz | */ + TRIGGER=1, /* Camera image triggered | */ + DISCONNECT=2, /* Camera connection lost | */ + ERROR=3, /* Camera unknown error | */ + LOWBATT=4, /* Camera battery low. Parameter p1 shows reported voltage | */ + LOWSTORE=5, /* Camera storage low. Parameter p1 shows reported shots remaining | */ + LOWSTOREV=6, /* Camera storage low. Parameter p1 shows reported video minutes remaining | */ + CAMERA_EVENT_TYPES_ENUM_END=7, /* | */ +} CAMERA_EVENT_TYPES; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_CAMERA_FEEDBACK_FLAGS +#define HAVE_ENUM_CAMERA_FEEDBACK_FLAGS +typedef enum CAMERA_FEEDBACK_FLAGS +{ + VIDEO=1, /* Shooting video, not stills | */ + BADEXPOSURE=2, /* Unable to achieve requested exposure (e.g. shutter speed too low) | */ + CAMERA_FEEDBACK_FLAGS_ENUM_END=3, /* | */ +} CAMERA_FEEDBACK_FLAGS; #endif #include "../common/common.h" @@ -122,6 +220,8 @@ enum PARACHUTE_ACTION #include "./mavlink_msg_rally_fetch_point.h" #include "./mavlink_msg_compassmot_status.h" #include "./mavlink_msg_ahrs2.h" +#include "./mavlink_msg_camera_event.h" +#include "./mavlink_msg_camera_feedback.h" #ifdef __cplusplus } diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_camera_event.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_camera_event.h new file mode 100644 index 000000000..82e7b3073 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_camera_event.h @@ -0,0 +1,401 @@ +// MESSAGE CAMERA_EVENT PACKING + +#define MAVLINK_MSG_ID_CAMERA_EVENT 179 + +typedef struct __mavlink_camera_event_t +{ + uint64_t time_usec; ///< Image timestamp (microseconds since UNIX epoch, according to camera clock) + float p1; ///< Parameter 1 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + float p2; ///< Parameter 2 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + float p3; ///< Parameter 3 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + float p4; ///< Parameter 4 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + uint16_t img_idx; ///< Image index + uint8_t target_system; ///< System ID + uint8_t cam_idx; ///< Camera ID + uint8_t event_id; ///< See CAMERA_EVENT_TYPES enum for definition of the bitmask +} mavlink_camera_event_t; + +#define MAVLINK_MSG_ID_CAMERA_EVENT_LEN 29 +#define MAVLINK_MSG_ID_179_LEN 29 + +#define MAVLINK_MSG_ID_CAMERA_EVENT_CRC 206 +#define MAVLINK_MSG_ID_179_CRC 206 + + + +#define MAVLINK_MESSAGE_INFO_CAMERA_EVENT { \ + "CAMERA_EVENT", \ + 9, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_camera_event_t, time_usec) }, \ + { "p1", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_event_t, p1) }, \ + { "p2", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_event_t, p2) }, \ + { "p3", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_event_t, p3) }, \ + { "p4", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_event_t, p4) }, \ + { "img_idx", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_camera_event_t, img_idx) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_camera_event_t, target_system) }, \ + { "cam_idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_camera_event_t, cam_idx) }, \ + { "event_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_camera_event_t, event_id) }, \ + } \ +} + + +/** + * @brief Pack a camera_event message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Image timestamp (microseconds since UNIX epoch, according to camera clock) + * @param target_system System ID + * @param cam_idx Camera ID + * @param img_idx Image index + * @param event_id See CAMERA_EVENT_TYPES enum for definition of the bitmask + * @param p1 Parameter 1 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + * @param p2 Parameter 2 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + * @param p3 Parameter 3 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + * @param p4 Parameter 4 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_event_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t target_system, uint8_t cam_idx, uint16_t img_idx, uint8_t event_id, float p1, float p2, float p3, float p4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_EVENT_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, p1); + _mav_put_float(buf, 12, p2); + _mav_put_float(buf, 16, p3); + _mav_put_float(buf, 20, p4); + _mav_put_uint16_t(buf, 24, img_idx); + _mav_put_uint8_t(buf, 26, target_system); + _mav_put_uint8_t(buf, 27, cam_idx); + _mav_put_uint8_t(buf, 28, event_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_EVENT_LEN); +#else + mavlink_camera_event_t packet; + packet.time_usec = time_usec; + packet.p1 = p1; + packet.p2 = p2; + packet.p3 = p3; + packet.p4 = p4; + packet.img_idx = img_idx; + packet.target_system = target_system; + packet.cam_idx = cam_idx; + packet.event_id = event_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_EVENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_EVENT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_EVENT_LEN, MAVLINK_MSG_ID_CAMERA_EVENT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_EVENT_LEN); +#endif +} + +/** + * @brief Pack a camera_event message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Image timestamp (microseconds since UNIX epoch, according to camera clock) + * @param target_system System ID + * @param cam_idx Camera ID + * @param img_idx Image index + * @param event_id See CAMERA_EVENT_TYPES enum for definition of the bitmask + * @param p1 Parameter 1 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + * @param p2 Parameter 2 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + * @param p3 Parameter 3 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + * @param p4 Parameter 4 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_event_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t target_system,uint8_t cam_idx,uint16_t img_idx,uint8_t event_id,float p1,float p2,float p3,float p4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_EVENT_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, p1); + _mav_put_float(buf, 12, p2); + _mav_put_float(buf, 16, p3); + _mav_put_float(buf, 20, p4); + _mav_put_uint16_t(buf, 24, img_idx); + _mav_put_uint8_t(buf, 26, target_system); + _mav_put_uint8_t(buf, 27, cam_idx); + _mav_put_uint8_t(buf, 28, event_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_EVENT_LEN); +#else + mavlink_camera_event_t packet; + packet.time_usec = time_usec; + packet.p1 = p1; + packet.p2 = p2; + packet.p3 = p3; + packet.p4 = p4; + packet.img_idx = img_idx; + packet.target_system = target_system; + packet.cam_idx = cam_idx; + packet.event_id = event_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_EVENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_EVENT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_EVENT_LEN, MAVLINK_MSG_ID_CAMERA_EVENT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_EVENT_LEN); +#endif +} + +/** + * @brief Encode a camera_event struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_event C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_event_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_event_t* camera_event) +{ + return mavlink_msg_camera_event_pack(system_id, component_id, msg, camera_event->time_usec, camera_event->target_system, camera_event->cam_idx, camera_event->img_idx, camera_event->event_id, camera_event->p1, camera_event->p2, camera_event->p3, camera_event->p4); +} + +/** + * @brief Encode a camera_event struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_event C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_event_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_event_t* camera_event) +{ + return mavlink_msg_camera_event_pack_chan(system_id, component_id, chan, msg, camera_event->time_usec, camera_event->target_system, camera_event->cam_idx, camera_event->img_idx, camera_event->event_id, camera_event->p1, camera_event->p2, camera_event->p3, camera_event->p4); +} + +/** + * @brief Send a camera_event message + * @param chan MAVLink channel to send the message + * + * @param time_usec Image timestamp (microseconds since UNIX epoch, according to camera clock) + * @param target_system System ID + * @param cam_idx Camera ID + * @param img_idx Image index + * @param event_id See CAMERA_EVENT_TYPES enum for definition of the bitmask + * @param p1 Parameter 1 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + * @param p2 Parameter 2 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + * @param p3 Parameter 3 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + * @param p4 Parameter 4 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_event_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t target_system, uint8_t cam_idx, uint16_t img_idx, uint8_t event_id, float p1, float p2, float p3, float p4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_EVENT_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, p1); + _mav_put_float(buf, 12, p2); + _mav_put_float(buf, 16, p3); + _mav_put_float(buf, 20, p4); + _mav_put_uint16_t(buf, 24, img_idx); + _mav_put_uint8_t(buf, 26, target_system); + _mav_put_uint8_t(buf, 27, cam_idx); + _mav_put_uint8_t(buf, 28, event_id); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_EVENT, buf, MAVLINK_MSG_ID_CAMERA_EVENT_LEN, MAVLINK_MSG_ID_CAMERA_EVENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_EVENT, buf, MAVLINK_MSG_ID_CAMERA_EVENT_LEN); +#endif +#else + mavlink_camera_event_t packet; + packet.time_usec = time_usec; + packet.p1 = p1; + packet.p2 = p2; + packet.p3 = p3; + packet.p4 = p4; + packet.img_idx = img_idx; + packet.target_system = target_system; + packet.cam_idx = cam_idx; + packet.event_id = event_id; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_EVENT, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_EVENT_LEN, MAVLINK_MSG_ID_CAMERA_EVENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_EVENT, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_EVENT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_EVENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_event_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t target_system, uint8_t cam_idx, uint16_t img_idx, uint8_t event_id, float p1, float p2, float p3, float p4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, p1); + _mav_put_float(buf, 12, p2); + _mav_put_float(buf, 16, p3); + _mav_put_float(buf, 20, p4); + _mav_put_uint16_t(buf, 24, img_idx); + _mav_put_uint8_t(buf, 26, target_system); + _mav_put_uint8_t(buf, 27, cam_idx); + _mav_put_uint8_t(buf, 28, event_id); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_EVENT, buf, MAVLINK_MSG_ID_CAMERA_EVENT_LEN, MAVLINK_MSG_ID_CAMERA_EVENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_EVENT, buf, MAVLINK_MSG_ID_CAMERA_EVENT_LEN); +#endif +#else + mavlink_camera_event_t *packet = (mavlink_camera_event_t *)msgbuf; + packet->time_usec = time_usec; + packet->p1 = p1; + packet->p2 = p2; + packet->p3 = p3; + packet->p4 = p4; + packet->img_idx = img_idx; + packet->target_system = target_system; + packet->cam_idx = cam_idx; + packet->event_id = event_id; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_EVENT, (const char *)packet, MAVLINK_MSG_ID_CAMERA_EVENT_LEN, MAVLINK_MSG_ID_CAMERA_EVENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_EVENT, (const char *)packet, MAVLINK_MSG_ID_CAMERA_EVENT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_EVENT UNPACKING + + +/** + * @brief Get field time_usec from camera_event message + * + * @return Image timestamp (microseconds since UNIX epoch, according to camera clock) + */ +static inline uint64_t mavlink_msg_camera_event_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field target_system from camera_event message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_camera_event_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 26); +} + +/** + * @brief Get field cam_idx from camera_event message + * + * @return Camera ID + */ +static inline uint8_t mavlink_msg_camera_event_get_cam_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 27); +} + +/** + * @brief Get field img_idx from camera_event message + * + * @return Image index + */ +static inline uint16_t mavlink_msg_camera_event_get_img_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field event_id from camera_event message + * + * @return See CAMERA_EVENT_TYPES enum for definition of the bitmask + */ +static inline uint8_t mavlink_msg_camera_event_get_event_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 28); +} + +/** + * @brief Get field p1 from camera_event message + * + * @return Parameter 1 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + */ +static inline float mavlink_msg_camera_event_get_p1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field p2 from camera_event message + * + * @return Parameter 2 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + */ +static inline float mavlink_msg_camera_event_get_p2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field p3 from camera_event message + * + * @return Parameter 3 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + */ +static inline float mavlink_msg_camera_event_get_p3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field p4 from camera_event message + * + * @return Parameter 4 (meaning depends on event, see CAMERA_EVENT_TYPES enum) + */ +static inline float mavlink_msg_camera_event_get_p4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Decode a camera_event message into a struct + * + * @param msg The message to decode + * @param camera_event C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_event_decode(const mavlink_message_t* msg, mavlink_camera_event_t* camera_event) +{ +#if MAVLINK_NEED_BYTE_SWAP + camera_event->time_usec = mavlink_msg_camera_event_get_time_usec(msg); + camera_event->p1 = mavlink_msg_camera_event_get_p1(msg); + camera_event->p2 = mavlink_msg_camera_event_get_p2(msg); + camera_event->p3 = mavlink_msg_camera_event_get_p3(msg); + camera_event->p4 = mavlink_msg_camera_event_get_p4(msg); + camera_event->img_idx = mavlink_msg_camera_event_get_img_idx(msg); + camera_event->target_system = mavlink_msg_camera_event_get_target_system(msg); + camera_event->cam_idx = mavlink_msg_camera_event_get_cam_idx(msg); + camera_event->event_id = mavlink_msg_camera_event_get_event_id(msg); +#else + memcpy(camera_event, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CAMERA_EVENT_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_camera_feedback.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_camera_feedback.h new file mode 100644 index 000000000..a70029c56 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_camera_feedback.h @@ -0,0 +1,473 @@ +// MESSAGE CAMERA_FEEDBACK PACKING + +#define MAVLINK_MSG_ID_CAMERA_FEEDBACK 180 + +typedef struct __mavlink_camera_feedback_t +{ + uint64_t time_usec; ///< Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_EVENT message + int32_t lat; ///< Latitude in (deg * 1E7) + int32_t lng; ///< Longitude in (deg * 1E7) + float alt; ///< Altitude (meters, AMSL) + float roll; ///< Vehicle Roll angle (degrees, +-180) + float pitch; ///< Vehicle Pitch angle (degrees, +-180) + float yaw; ///< Vehicle Heading (degrees, 0-360, true) + float foc_len; ///< Focal Length (mm) + uint16_t img_idx; ///< Image index + uint8_t target_system; ///< System ID + uint8_t cam_idx; ///< Camera ID + uint8_t flags; ///< See CAMERA_FEEDBACK_FLAGS enum for definition of the bitmask +} mavlink_camera_feedback_t; + +#define MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN 41 +#define MAVLINK_MSG_ID_180_LEN 41 + +#define MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC 151 +#define MAVLINK_MSG_ID_180_CRC 151 + + + +#define MAVLINK_MESSAGE_INFO_CAMERA_FEEDBACK { \ + "CAMERA_FEEDBACK", \ + 12, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_camera_feedback_t, time_usec) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_camera_feedback_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_camera_feedback_t, lng) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_feedback_t, alt) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_feedback_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_feedback_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_camera_feedback_t, yaw) }, \ + { "foc_len", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_camera_feedback_t, foc_len) }, \ + { "img_idx", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_camera_feedback_t, img_idx) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_camera_feedback_t, target_system) }, \ + { "cam_idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 39, offsetof(mavlink_camera_feedback_t, cam_idx) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_camera_feedback_t, flags) }, \ + } \ +} + + +/** + * @brief Pack a camera_feedback message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_EVENT message + * @param target_system System ID + * @param cam_idx Camera ID + * @param img_idx Image index + * @param lat Latitude in (deg * 1E7) + * @param lng Longitude in (deg * 1E7) + * @param alt Altitude (meters, AMSL) + * @param roll Vehicle Roll angle (degrees, +-180) + * @param pitch Vehicle Pitch angle (degrees, +-180) + * @param yaw Vehicle Heading (degrees, 0-360, true) + * @param foc_len Focal Length (mm) + * @param flags See CAMERA_FEEDBACK_FLAGS enum for definition of the bitmask + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_feedback_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t target_system, uint8_t cam_idx, uint16_t img_idx, int32_t lat, int32_t lng, float alt, float roll, float pitch, float yaw, float foc_len, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lng); + _mav_put_float(buf, 16, alt); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + _mav_put_float(buf, 32, foc_len); + _mav_put_uint16_t(buf, 36, img_idx); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, cam_idx); + _mav_put_uint8_t(buf, 40, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#else + mavlink_camera_feedback_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lng = lng; + packet.alt = alt; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.foc_len = foc_len; + packet.img_idx = img_idx; + packet.target_system = target_system; + packet.cam_idx = cam_idx; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_FEEDBACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN, MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +} + +/** + * @brief Pack a camera_feedback message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_EVENT message + * @param target_system System ID + * @param cam_idx Camera ID + * @param img_idx Image index + * @param lat Latitude in (deg * 1E7) + * @param lng Longitude in (deg * 1E7) + * @param alt Altitude (meters, AMSL) + * @param roll Vehicle Roll angle (degrees, +-180) + * @param pitch Vehicle Pitch angle (degrees, +-180) + * @param yaw Vehicle Heading (degrees, 0-360, true) + * @param foc_len Focal Length (mm) + * @param flags See CAMERA_FEEDBACK_FLAGS enum for definition of the bitmask + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_feedback_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t target_system,uint8_t cam_idx,uint16_t img_idx,int32_t lat,int32_t lng,float alt,float roll,float pitch,float yaw,float foc_len,uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lng); + _mav_put_float(buf, 16, alt); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + _mav_put_float(buf, 32, foc_len); + _mav_put_uint16_t(buf, 36, img_idx); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, cam_idx); + _mav_put_uint8_t(buf, 40, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#else + mavlink_camera_feedback_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lng = lng; + packet.alt = alt; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.foc_len = foc_len; + packet.img_idx = img_idx; + packet.target_system = target_system; + packet.cam_idx = cam_idx; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_FEEDBACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN, MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +} + +/** + * @brief Encode a camera_feedback struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_feedback C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_feedback_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_feedback_t* camera_feedback) +{ + return mavlink_msg_camera_feedback_pack(system_id, component_id, msg, camera_feedback->time_usec, camera_feedback->target_system, camera_feedback->cam_idx, camera_feedback->img_idx, camera_feedback->lat, camera_feedback->lng, camera_feedback->alt, camera_feedback->roll, camera_feedback->pitch, camera_feedback->yaw, camera_feedback->foc_len, camera_feedback->flags); +} + +/** + * @brief Encode a camera_feedback struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_feedback C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_feedback_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_feedback_t* camera_feedback) +{ + return mavlink_msg_camera_feedback_pack_chan(system_id, component_id, chan, msg, camera_feedback->time_usec, camera_feedback->target_system, camera_feedback->cam_idx, camera_feedback->img_idx, camera_feedback->lat, camera_feedback->lng, camera_feedback->alt, camera_feedback->roll, camera_feedback->pitch, camera_feedback->yaw, camera_feedback->foc_len, camera_feedback->flags); +} + +/** + * @brief Send a camera_feedback message + * @param chan MAVLink channel to send the message + * + * @param time_usec Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_EVENT message + * @param target_system System ID + * @param cam_idx Camera ID + * @param img_idx Image index + * @param lat Latitude in (deg * 1E7) + * @param lng Longitude in (deg * 1E7) + * @param alt Altitude (meters, AMSL) + * @param roll Vehicle Roll angle (degrees, +-180) + * @param pitch Vehicle Pitch angle (degrees, +-180) + * @param yaw Vehicle Heading (degrees, 0-360, true) + * @param foc_len Focal Length (mm) + * @param flags See CAMERA_FEEDBACK_FLAGS enum for definition of the bitmask + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_feedback_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t target_system, uint8_t cam_idx, uint16_t img_idx, int32_t lat, int32_t lng, float alt, float roll, float pitch, float yaw, float foc_len, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lng); + _mav_put_float(buf, 16, alt); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + _mav_put_float(buf, 32, foc_len); + _mav_put_uint16_t(buf, 36, img_idx); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, cam_idx); + _mav_put_uint8_t(buf, 40, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, buf, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN, MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, buf, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +#else + mavlink_camera_feedback_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lng = lng; + packet.alt = alt; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.foc_len = foc_len; + packet.img_idx = img_idx; + packet.target_system = target_system; + packet.cam_idx = cam_idx; + packet.flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN, MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_feedback_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t target_system, uint8_t cam_idx, uint16_t img_idx, int32_t lat, int32_t lng, float alt, float roll, float pitch, float yaw, float foc_len, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lng); + _mav_put_float(buf, 16, alt); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + _mav_put_float(buf, 32, foc_len); + _mav_put_uint16_t(buf, 36, img_idx); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, cam_idx); + _mav_put_uint8_t(buf, 40, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, buf, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN, MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, buf, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +#else + mavlink_camera_feedback_t *packet = (mavlink_camera_feedback_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lng = lng; + packet->alt = alt; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->foc_len = foc_len; + packet->img_idx = img_idx; + packet->target_system = target_system; + packet->cam_idx = cam_idx; + packet->flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, (const char *)packet, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN, MAVLINK_MSG_ID_CAMERA_FEEDBACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FEEDBACK, (const char *)packet, MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_FEEDBACK UNPACKING + + +/** + * @brief Get field time_usec from camera_feedback message + * + * @return Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_EVENT message + */ +static inline uint64_t mavlink_msg_camera_feedback_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field target_system from camera_feedback message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_camera_feedback_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 38); +} + +/** + * @brief Get field cam_idx from camera_feedback message + * + * @return Camera ID + */ +static inline uint8_t mavlink_msg_camera_feedback_get_cam_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 39); +} + +/** + * @brief Get field img_idx from camera_feedback message + * + * @return Image index + */ +static inline uint16_t mavlink_msg_camera_feedback_get_img_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 36); +} + +/** + * @brief Get field lat from camera_feedback message + * + * @return Latitude in (deg * 1E7) + */ +static inline int32_t mavlink_msg_camera_feedback_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field lng from camera_feedback message + * + * @return Longitude in (deg * 1E7) + */ +static inline int32_t mavlink_msg_camera_feedback_get_lng(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field alt from camera_feedback message + * + * @return Altitude (meters, AMSL) + */ +static inline float mavlink_msg_camera_feedback_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field roll from camera_feedback message + * + * @return Vehicle Roll angle (degrees, +-180) + */ +static inline float mavlink_msg_camera_feedback_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field pitch from camera_feedback message + * + * @return Vehicle Pitch angle (degrees, +-180) + */ +static inline float mavlink_msg_camera_feedback_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field yaw from camera_feedback message + * + * @return Vehicle Heading (degrees, 0-360, true) + */ +static inline float mavlink_msg_camera_feedback_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field foc_len from camera_feedback message + * + * @return Focal Length (mm) + */ +static inline float mavlink_msg_camera_feedback_get_foc_len(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field flags from camera_feedback message + * + * @return See CAMERA_FEEDBACK_FLAGS enum for definition of the bitmask + */ +static inline uint8_t mavlink_msg_camera_feedback_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Decode a camera_feedback message into a struct + * + * @param msg The message to decode + * @param camera_feedback C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_feedback_decode(const mavlink_message_t* msg, mavlink_camera_feedback_t* camera_feedback) +{ +#if MAVLINK_NEED_BYTE_SWAP + camera_feedback->time_usec = mavlink_msg_camera_feedback_get_time_usec(msg); + camera_feedback->lat = mavlink_msg_camera_feedback_get_lat(msg); + camera_feedback->lng = mavlink_msg_camera_feedback_get_lng(msg); + camera_feedback->alt = mavlink_msg_camera_feedback_get_alt(msg); + camera_feedback->roll = mavlink_msg_camera_feedback_get_roll(msg); + camera_feedback->pitch = mavlink_msg_camera_feedback_get_pitch(msg); + camera_feedback->yaw = mavlink_msg_camera_feedback_get_yaw(msg); + camera_feedback->foc_len = mavlink_msg_camera_feedback_get_foc_len(msg); + camera_feedback->img_idx = mavlink_msg_camera_feedback_get_img_idx(msg); + camera_feedback->target_system = mavlink_msg_camera_feedback_get_target_system(msg); + camera_feedback->cam_idx = mavlink_msg_camera_feedback_get_cam_idx(msg); + camera_feedback->flags = mavlink_msg_camera_feedback_get_flags(msg); +#else + memcpy(camera_feedback, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CAMERA_FEEDBACK_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h index 489553ea4..028a7d753 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h @@ -1504,6 +1504,130 @@ static void mavlink_test_ahrs2(uint8_t system_id, uint8_t component_id, mavlink_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_camera_event(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_event_t packet_in = { + 93372036854775807ULL, + }73.0, + }101.0, + }129.0, + }157.0, + }18483, + }211, + }22, + }89, + }; + mavlink_camera_event_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.p1 = packet_in.p1; + packet1.p2 = packet_in.p2; + packet1.p3 = packet_in.p3; + packet1.p4 = packet_in.p4; + packet1.img_idx = packet_in.img_idx; + packet1.target_system = packet_in.target_system; + packet1.cam_idx = packet_in.cam_idx; + packet1.event_id = packet_in.event_id; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_event_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_event_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_event_pack(system_id, component_id, &msg , packet1.time_usec , packet1.target_system , packet1.cam_idx , packet1.img_idx , packet1.event_id , packet1.p1 , packet1.p2 , packet1.p3 , packet1.p4 ); + mavlink_msg_camera_event_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_event_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.target_system , packet1.cam_idx , packet1.img_idx , packet1.event_id , packet1.p1 , packet1.p2 , packet1.p3 , packet1.p4 ); + mavlink_msg_camera_event_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ @@ -337,7 +334,7 @@ enum MAV_CMD MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Custom mode - this is system specific, please refer to the individual autopilot specifications for details.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ @@ -355,6 +352,7 @@ enum MAV_CMD MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ @@ -366,7 +364,7 @@ enum MAV_CMD MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ MAV_CMD_ENUM_END=501, /* | */ -}; +} MAV_CMD; #endif /** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a @@ -374,7 +372,7 @@ enum MAV_CMD the recommended messages. */ #ifndef HAVE_ENUM_MAV_DATA_STREAM #define HAVE_ENUM_MAV_DATA_STREAM -enum MAV_DATA_STREAM +typedef enum MAV_DATA_STREAM { MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */ @@ -386,7 +384,7 @@ enum MAV_DATA_STREAM MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ MAV_DATA_STREAM_ENUM_END=13, /* | */ -}; +} MAV_DATA_STREAM; #endif /** @brief The ROI (region of interest) for the vehicle. This can be @@ -394,7 +392,7 @@ enum MAV_DATA_STREAM MAV_CMD_NAV_ROI). */ #ifndef HAVE_ENUM_MAV_ROI #define HAVE_ENUM_MAV_ROI -enum MAV_ROI +typedef enum MAV_ROI { MAV_ROI_NONE=0, /* No region of interest. | */ MAV_ROI_WPNEXT=1, /* Point toward next MISSION. | */ @@ -402,13 +400,13 @@ enum MAV_ROI MAV_ROI_LOCATION=3, /* Point toward fixed location. | */ MAV_ROI_TARGET=4, /* Point toward of given id. | */ MAV_ROI_ENUM_END=5, /* | */ -}; +} MAV_ROI; #endif /** @brief ACK / NACK / ERROR values as a result of MAV_CMDs and for mission item transmission. */ #ifndef HAVE_ENUM_MAV_CMD_ACK #define HAVE_ENUM_MAV_CMD_ACK -enum MAV_CMD_ACK +typedef enum MAV_CMD_ACK { MAV_CMD_ACK_OK=1, /* Command / mission item is ok. | */ MAV_CMD_ACK_ERR_FAIL=2, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. | */ @@ -420,13 +418,13 @@ enum MAV_CMD_ACK MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=8, /* The Y or longitude value is out of range. | */ MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=9, /* The Z or altitude value is out of range. | */ MAV_CMD_ACK_ENUM_END=10, /* | */ -}; +} MAV_CMD_ACK; #endif /** @brief Specifies the datatype of a MAVLink parameter. */ #ifndef HAVE_ENUM_MAV_PARAM_TYPE #define HAVE_ENUM_MAV_PARAM_TYPE -enum MAV_PARAM_TYPE +typedef enum MAV_PARAM_TYPE { MAV_PARAM_TYPE_UINT8=1, /* 8-bit unsigned integer | */ MAV_PARAM_TYPE_INT8=2, /* 8-bit signed integer | */ @@ -439,13 +437,13 @@ enum MAV_PARAM_TYPE MAV_PARAM_TYPE_REAL32=9, /* 32-bit floating-point | */ MAV_PARAM_TYPE_REAL64=10, /* 64-bit floating-point | */ MAV_PARAM_TYPE_ENUM_END=11, /* | */ -}; +} MAV_PARAM_TYPE; #endif /** @brief result from a mavlink command */ #ifndef HAVE_ENUM_MAV_RESULT #define HAVE_ENUM_MAV_RESULT -enum MAV_RESULT +typedef enum MAV_RESULT { MAV_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ MAV_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */ @@ -453,13 +451,13 @@ enum MAV_RESULT MAV_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */ MAV_RESULT_FAILED=4, /* Command executed, but failed | */ MAV_RESULT_ENUM_END=5, /* | */ -}; +} MAV_RESULT; #endif /** @brief result in a mavlink mission ack */ #ifndef HAVE_ENUM_MAV_MISSION_RESULT #define HAVE_ENUM_MAV_MISSION_RESULT -enum MAV_MISSION_RESULT +typedef enum MAV_MISSION_RESULT { MAV_MISSION_ACCEPTED=0, /* mission accepted OK | */ MAV_MISSION_ERROR=1, /* generic error / not accepting mission commands at all right now | */ @@ -477,13 +475,13 @@ enum MAV_MISSION_RESULT MAV_MISSION_INVALID_SEQUENCE=13, /* received waypoint out of sequence | */ MAV_MISSION_DENIED=14, /* not accepting any mission commands from this communication partner | */ MAV_MISSION_RESULT_ENUM_END=15, /* | */ -}; +} MAV_MISSION_RESULT; #endif /** @brief Indicates the severity level, generally used for status messages to indicate their relative urgency. Based on RFC-5424 using expanded definitions at: http://www.kiwisyslog.com/kb/info:-syslog-message-levels/. */ #ifndef HAVE_ENUM_MAV_SEVERITY #define HAVE_ENUM_MAV_SEVERITY -enum MAV_SEVERITY +typedef enum MAV_SEVERITY { MAV_SEVERITY_EMERGENCY=0, /* System is unusable. This is a "panic" condition. | */ MAV_SEVERITY_ALERT=1, /* Action should be taken immediately. Indicates error in non-critical systems. | */ @@ -494,13 +492,13 @@ enum MAV_SEVERITY MAV_SEVERITY_INFO=6, /* Normal operational messages. Useful for logging. No action is required for these messages. | */ MAV_SEVERITY_DEBUG=7, /* Useful non-operational messages that can assist in debugging. These should not occur during normal operation. | */ MAV_SEVERITY_ENUM_END=8, /* | */ -}; +} MAV_SEVERITY; #endif /** @brief Power supply status flags (bitmask) */ #ifndef HAVE_ENUM_MAV_POWER_STATUS #define HAVE_ENUM_MAV_POWER_STATUS -enum MAV_POWER_STATUS +typedef enum MAV_POWER_STATUS { MAV_POWER_STATUS_BRICK_VALID=1, /* main brick power supply valid | */ MAV_POWER_STATUS_SERVO_VALID=2, /* main servo power supply valid for FMU | */ @@ -509,26 +507,26 @@ enum MAV_POWER_STATUS MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT=16, /* hi-power peripheral supply is in over-current state | */ MAV_POWER_STATUS_CHANGED=32, /* Power status has changed since boot | */ MAV_POWER_STATUS_ENUM_END=33, /* | */ -}; +} MAV_POWER_STATUS; #endif /** @brief SERIAL_CONTROL device types */ #ifndef HAVE_ENUM_SERIAL_CONTROL_DEV #define HAVE_ENUM_SERIAL_CONTROL_DEV -enum SERIAL_CONTROL_DEV +typedef enum SERIAL_CONTROL_DEV { SERIAL_CONTROL_DEV_TELEM1=0, /* First telemetry port | */ SERIAL_CONTROL_DEV_TELEM2=1, /* Second telemetry port | */ SERIAL_CONTROL_DEV_GPS1=2, /* First GPS port | */ SERIAL_CONTROL_DEV_GPS2=3, /* Second GPS port | */ SERIAL_CONTROL_DEV_ENUM_END=4, /* | */ -}; +} SERIAL_CONTROL_DEV; #endif /** @brief SERIAL_CONTROL flags (bitmask) */ #ifndef HAVE_ENUM_SERIAL_CONTROL_FLAG #define HAVE_ENUM_SERIAL_CONTROL_FLAG -enum SERIAL_CONTROL_FLAG +typedef enum SERIAL_CONTROL_FLAG { SERIAL_CONTROL_FLAG_REPLY=1, /* Set if this is a reply | */ SERIAL_CONTROL_FLAG_RESPOND=2, /* Set if the sender wants the receiver to send a response as another SERIAL_CONTROL message | */ @@ -536,7 +534,18 @@ enum SERIAL_CONTROL_FLAG SERIAL_CONTROL_FLAG_BLOCKING=8, /* Block on writes to the serial port | */ SERIAL_CONTROL_FLAG_MULTI=16, /* Send multiple replies until port is drained | */ SERIAL_CONTROL_FLAG_ENUM_END=17, /* | */ -}; +} SERIAL_CONTROL_FLAG; +#endif + +/** @brief Enumeration of distance sensor types */ +#ifndef HAVE_ENUM_MAV_DISTANCE_SENSOR +#define HAVE_ENUM_MAV_DISTANCE_SENSOR +typedef enum MAV_DISTANCE_SENSOR +{ + MAV_DISTANCE_SENSOR_LASER=0, /* Laser altimeter, e.g. LightWare SF02/F or PulsedLight units | */ + MAV_DISTANCE_SENSOR_ULTRASOUND=1, /* Ultrasound altimeter, e.g. MaxBotix units | */ + MAV_DISTANCE_SENSOR_ENUM_END=2, /* | */ +} MAV_DISTANCE_SENSOR; #endif @@ -616,6 +625,9 @@ enum SERIAL_CONTROL_FLAG #include "./mavlink_msg_command_ack.h" #include "./mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h" #include "./mavlink_msg_manual_setpoint.h" +#include "./mavlink_msg_attitude_setpoint_external.h" +#include "./mavlink_msg_local_ned_position_setpoint_external.h" +#include "./mavlink_msg_global_position_setpoint_external_int.h" #include "./mavlink_msg_local_position_ned_system_global_offset.h" #include "./mavlink_msg_hil_state.h" #include "./mavlink_msg_hil_controls.h" diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_setpoint_external.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_setpoint_external.h new file mode 100644 index 000000000..d50179f45 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_setpoint_external.h @@ -0,0 +1,393 @@ +// MESSAGE ATTITUDE_SETPOINT_EXTERNAL PACKING + +#define MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL 82 + +typedef struct __mavlink_attitude_setpoint_external_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot + float q[4]; ///< Quaternion + float body_roll_rate; ///< Body roll rate in radians per second + float body_pitch_rate; ///< Body roll rate in radians per second + float body_yaw_rate; ///< Body roll rate in radians per second + float thrust; ///< Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t ignore_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: roll, bit 2: pitch, bit 3: yaw, bit 4: roll rate, bit 5: pitch rate, bit 6: yaw rate, bit 7: thrust +} mavlink_attitude_setpoint_external_t; + +#define MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN 39 +#define MAVLINK_MSG_ID_82_LEN 39 + +#define MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_CRC 212 +#define MAVLINK_MSG_ID_82_CRC 212 + +#define MAVLINK_MSG_ATTITUDE_SETPOINT_EXTERNAL_FIELD_Q_LEN 4 + +#define MAVLINK_MESSAGE_INFO_ATTITUDE_SETPOINT_EXTERNAL { \ + "ATTITUDE_SETPOINT_EXTERNAL", \ + 9, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_setpoint_external_t, time_boot_ms) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_attitude_setpoint_external_t, q) }, \ + { "body_roll_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_setpoint_external_t, body_roll_rate) }, \ + { "body_pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_setpoint_external_t, body_pitch_rate) }, \ + { "body_yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_setpoint_external_t, body_yaw_rate) }, \ + { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_attitude_setpoint_external_t, thrust) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_attitude_setpoint_external_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_attitude_setpoint_external_t, target_component) }, \ + { "ignore_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_attitude_setpoint_external_t, ignore_mask) }, \ + } \ +} + + +/** + * @brief Pack a attitude_setpoint_external message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: roll, bit 2: pitch, bit 3: yaw, bit 4: roll rate, bit 5: pitch rate, bit 6: yaw rate, bit 7: thrust + * @param q Quaternion + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_setpoint_external_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t ignore_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, ignore_mask); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN); +#else + mavlink_attitude_setpoint_external_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + packet.ignore_mask = ignore_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN); +#endif +} + +/** + * @brief Pack a attitude_setpoint_external message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: roll, bit 2: pitch, bit 3: yaw, bit 4: roll rate, bit 5: pitch rate, bit 6: yaw rate, bit 7: thrust + * @param q Quaternion + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_setpoint_external_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t ignore_mask,const float *q,float body_roll_rate,float body_pitch_rate,float body_yaw_rate,float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, ignore_mask); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN); +#else + mavlink_attitude_setpoint_external_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + packet.ignore_mask = ignore_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN); +#endif +} + +/** + * @brief Encode a attitude_setpoint_external struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param attitude_setpoint_external C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_setpoint_external_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_setpoint_external_t* attitude_setpoint_external) +{ + return mavlink_msg_attitude_setpoint_external_pack(system_id, component_id, msg, attitude_setpoint_external->time_boot_ms, attitude_setpoint_external->target_system, attitude_setpoint_external->target_component, attitude_setpoint_external->ignore_mask, attitude_setpoint_external->q, attitude_setpoint_external->body_roll_rate, attitude_setpoint_external->body_pitch_rate, attitude_setpoint_external->body_yaw_rate, attitude_setpoint_external->thrust); +} + +/** + * @brief Encode a attitude_setpoint_external struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude_setpoint_external C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_setpoint_external_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_setpoint_external_t* attitude_setpoint_external) +{ + return mavlink_msg_attitude_setpoint_external_pack_chan(system_id, component_id, chan, msg, attitude_setpoint_external->time_boot_ms, attitude_setpoint_external->target_system, attitude_setpoint_external->target_component, attitude_setpoint_external->ignore_mask, attitude_setpoint_external->q, attitude_setpoint_external->body_roll_rate, attitude_setpoint_external->body_pitch_rate, attitude_setpoint_external->body_yaw_rate, attitude_setpoint_external->thrust); +} + +/** + * @brief Send a attitude_setpoint_external message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: roll, bit 2: pitch, bit 3: yaw, bit 4: roll rate, bit 5: pitch rate, bit 6: yaw rate, bit 7: thrust + * @param q Quaternion + * @param body_roll_rate Body roll rate in radians per second + * @param body_pitch_rate Body roll rate in radians per second + * @param body_yaw_rate Body roll rate in radians per second + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_attitude_setpoint_external_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t ignore_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, ignore_mask); + _mav_put_float_array(buf, 4, q, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL, buf, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL, buf, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN); +#endif +#else + mavlink_attitude_setpoint_external_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + packet.ignore_mask = ignore_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_setpoint_external_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t ignore_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, ignore_mask); + _mav_put_float_array(buf, 4, q, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL, buf, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL, buf, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN); +#endif +#else + mavlink_attitude_setpoint_external_t *packet = (mavlink_attitude_setpoint_external_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->body_roll_rate = body_roll_rate; + packet->body_pitch_rate = body_pitch_rate; + packet->body_yaw_rate = body_yaw_rate; + packet->thrust = thrust; + packet->target_system = target_system; + packet->target_component = target_component; + packet->ignore_mask = ignore_mask; + mav_array_memcpy(packet->q, q, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE ATTITUDE_SETPOINT_EXTERNAL UNPACKING + + +/** + * @brief Get field time_boot_ms from attitude_setpoint_external message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_attitude_setpoint_external_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field target_system from attitude_setpoint_external message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_attitude_setpoint_external_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field target_component from attitude_setpoint_external message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_attitude_setpoint_external_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 37); +} + +/** + * @brief Get field ignore_mask from attitude_setpoint_external message + * + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: roll, bit 2: pitch, bit 3: yaw, bit 4: roll rate, bit 5: pitch rate, bit 6: yaw rate, bit 7: thrust + */ +static inline uint8_t mavlink_msg_attitude_setpoint_external_get_ignore_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 38); +} + +/** + * @brief Get field q from attitude_setpoint_external message + * + * @return Quaternion + */ +static inline uint16_t mavlink_msg_attitude_setpoint_external_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 4); +} + +/** + * @brief Get field body_roll_rate from attitude_setpoint_external message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_attitude_setpoint_external_get_body_roll_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field body_pitch_rate from attitude_setpoint_external message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_attitude_setpoint_external_get_body_pitch_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field body_yaw_rate from attitude_setpoint_external message + * + * @return Body roll rate in radians per second + */ +static inline float mavlink_msg_attitude_setpoint_external_get_body_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field thrust from attitude_setpoint_external message + * + * @return Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + */ +static inline float mavlink_msg_attitude_setpoint_external_get_thrust(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Decode a attitude_setpoint_external message into a struct + * + * @param msg The message to decode + * @param attitude_setpoint_external C-struct to decode the message contents into + */ +static inline void mavlink_msg_attitude_setpoint_external_decode(const mavlink_message_t* msg, mavlink_attitude_setpoint_external_t* attitude_setpoint_external) +{ +#if MAVLINK_NEED_BYTE_SWAP + attitude_setpoint_external->time_boot_ms = mavlink_msg_attitude_setpoint_external_get_time_boot_ms(msg); + mavlink_msg_attitude_setpoint_external_get_q(msg, attitude_setpoint_external->q); + attitude_setpoint_external->body_roll_rate = mavlink_msg_attitude_setpoint_external_get_body_roll_rate(msg); + attitude_setpoint_external->body_pitch_rate = mavlink_msg_attitude_setpoint_external_get_body_pitch_rate(msg); + attitude_setpoint_external->body_yaw_rate = mavlink_msg_attitude_setpoint_external_get_body_yaw_rate(msg); + attitude_setpoint_external->thrust = mavlink_msg_attitude_setpoint_external_get_thrust(msg); + attitude_setpoint_external->target_system = mavlink_msg_attitude_setpoint_external_get_target_system(msg); + attitude_setpoint_external->target_component = mavlink_msg_attitude_setpoint_external_get_target_component(msg); + attitude_setpoint_external->ignore_mask = mavlink_msg_attitude_setpoint_external_get_ignore_mask(msg); +#else + memcpy(attitude_setpoint_external, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h index 39b384396..8743b6485 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h @@ -44,17 +44,17 @@ typedef struct __mavlink_distance_sensor_t * @param msg The MAVLink message to compress the data into * * @param time_boot_ms Time since system boot - * @param type Type from MAV_DISTANCE_SENSOR enum. - * @param id Onboard ID of the sensor - * @param orientation Direction the sensor faces from FIXME enum. * @param min_distance Minimum distance the sensor can measure in centimeters * @param max_distance Maximum distance the sensor can measure in centimeters * @param current_distance Current distance reading + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from FIXME enum. * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance) + uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; @@ -97,18 +97,18 @@ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8 * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Time since system boot - * @param type Type from MAV_DISTANCE_SENSOR enum. - * @param id Onboard ID of the sensor - * @param orientation Direction the sensor faces from FIXME enum. * @param min_distance Minimum distance the sensor can measure in centimeters * @param max_distance Maximum distance the sensor can measure in centimeters * @param current_distance Current distance reading + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from FIXME enum. * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t type,uint8_t id,uint8_t orientation,uint16_t min_distance,uint16_t max_distance,uint16_t current_distance,uint8_t covariance) + uint32_t time_boot_ms,uint16_t min_distance,uint16_t max_distance,uint16_t current_distance,uint8_t type,uint8_t id,uint8_t orientation,uint8_t covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; @@ -154,7 +154,7 @@ static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_distance_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) { - return mavlink_msg_distance_sensor_pack(system_id, component_id, msg, distance_sensor->time_boot_ms, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->covariance); + return mavlink_msg_distance_sensor_pack(system_id, component_id, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance); } /** @@ -168,7 +168,7 @@ static inline uint16_t mavlink_msg_distance_sensor_encode(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) { - return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->covariance); + return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance); } /** @@ -176,17 +176,17 @@ static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id * @param chan MAVLink channel to send the message * * @param time_boot_ms Time since system boot - * @param type Type from MAV_DISTANCE_SENSOR enum. - * @param id Onboard ID of the sensor - * @param orientation Direction the sensor faces from FIXME enum. * @param min_distance Minimum distance the sensor can measure in centimeters * @param max_distance Maximum distance the sensor can measure in centimeters * @param current_distance Current distance reading + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from FIXME enum. * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance) +static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; @@ -231,7 +231,7 @@ static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance) +static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -285,63 +285,63 @@ static inline uint32_t mavlink_msg_distance_sensor_get_time_boot_ms(const mavlin } /** - * @brief Get field type from distance_sensor message + * @brief Get field min_distance from distance_sensor message * - * @return Type from MAV_DISTANCE_SENSOR enum. + * @return Minimum distance the sensor can measure in centimeters */ -static inline uint8_t mavlink_msg_distance_sensor_get_type(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_distance_sensor_get_min_distance(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 10); + return _MAV_RETURN_uint16_t(msg, 4); } /** - * @brief Get field id from distance_sensor message + * @brief Get field max_distance from distance_sensor message * - * @return Onboard ID of the sensor + * @return Maximum distance the sensor can measure in centimeters */ -static inline uint8_t mavlink_msg_distance_sensor_get_id(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_distance_sensor_get_max_distance(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 11); + return _MAV_RETURN_uint16_t(msg, 6); } /** - * @brief Get field orientation from distance_sensor message + * @brief Get field current_distance from distance_sensor message * - * @return Direction the sensor faces from FIXME enum. + * @return Current distance reading */ -static inline uint8_t mavlink_msg_distance_sensor_get_orientation(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_distance_sensor_get_current_distance(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 12); + return _MAV_RETURN_uint16_t(msg, 8); } /** - * @brief Get field min_distance from distance_sensor message + * @brief Get field type from distance_sensor message * - * @return Minimum distance the sensor can measure in centimeters + * @return Type from MAV_DISTANCE_SENSOR enum. */ -static inline uint16_t mavlink_msg_distance_sensor_get_min_distance(const mavlink_message_t* msg) +static inline uint8_t mavlink_msg_distance_sensor_get_type(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 4); + return _MAV_RETURN_uint8_t(msg, 10); } /** - * @brief Get field max_distance from distance_sensor message + * @brief Get field id from distance_sensor message * - * @return Maximum distance the sensor can measure in centimeters + * @return Onboard ID of the sensor */ -static inline uint16_t mavlink_msg_distance_sensor_get_max_distance(const mavlink_message_t* msg) +static inline uint8_t mavlink_msg_distance_sensor_get_id(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 6); + return _MAV_RETURN_uint8_t(msg, 11); } /** - * @brief Get field current_distance from distance_sensor message + * @brief Get field orientation from distance_sensor message * - * @return Current distance reading + * @return Direction the sensor faces from FIXME enum. */ -static inline uint16_t mavlink_msg_distance_sensor_get_current_distance(const mavlink_message_t* msg) +static inline uint8_t mavlink_msg_distance_sensor_get_orientation(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 8); + return _MAV_RETURN_uint8_t(msg, 12); } /** diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_external_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_external_int.h new file mode 100644 index 000000000..46ab556d8 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_external_int.h @@ -0,0 +1,497 @@ +// MESSAGE GLOBAL_POSITION_SETPOINT_EXTERNAL_INT PACKING + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT 84 + +typedef struct __mavlink_global_position_setpoint_external_int_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + int32_t lat_int; ///< X Position in WGS84 frame in 1e7 * meters + int32_t lon_int; ///< Y Position in WGS84 frame in 1e7 * meters + float alt; ///< Altitude in WGS84, not AMSL + float vx; ///< X velocity in NED frame in meter / s + float vy; ///< Y velocity in NED frame in meter / s + float vz; ///< Z velocity in NED frame in meter / s + float ax; ///< X acceleration in NED frame in meter / s^2 + float ay; ///< Y acceleration in NED frame in meter / s^2 + float az; ///< Z acceleration in NED frame in meter / s^2 + uint16_t ignore_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: lat, bit 2: lon, bit 3: alt, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_global_position_setpoint_external_int_t; + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN 44 +#define MAVLINK_MSG_ID_84_LEN 44 + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_CRC 172 +#define MAVLINK_MSG_ID_84_CRC 172 + + + +#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT { \ + "GLOBAL_POSITION_SETPOINT_EXTERNAL_INT", \ + 13, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_global_position_setpoint_external_int_t, time_boot_ms) }, \ + { "lat_int", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_setpoint_external_int_t, lat_int) }, \ + { "lon_int", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_setpoint_external_int_t, lon_int) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_position_setpoint_external_int_t, alt) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_global_position_setpoint_external_int_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_position_setpoint_external_int_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_position_setpoint_external_int_t, vz) }, \ + { "ax", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_position_setpoint_external_int_t, ax) }, \ + { "ay", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_global_position_setpoint_external_int_t, ay) }, \ + { "az", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_global_position_setpoint_external_int_t, az) }, \ + { "ignore_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_global_position_setpoint_external_int_t, ignore_mask) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_global_position_setpoint_external_int_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_global_position_setpoint_external_int_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a global_position_setpoint_external_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param target_system System ID + * @param target_component Component ID + * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: lat, bit 2: lon, bit 3: alt, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in WGS84, not AMSL + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param ax X acceleration in NED frame in meter / s^2 + * @param ay Y acceleration in NED frame in meter / s^2 + * @param az Z acceleration in NED frame in meter / s^2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_setpoint_external_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint16_t ignore_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float ax, float ay, float az) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, ax); + _mav_put_float(buf, 32, ay); + _mav_put_float(buf, 36, az); + _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_uint8_t(buf, 42, target_system); + _mav_put_uint8_t(buf, 43, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN); +#else + mavlink_global_position_setpoint_external_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ax = ax; + packet.ay = ay; + packet.az = az; + packet.ignore_mask = ignore_mask; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN); +#endif +} + +/** + * @brief Pack a global_position_setpoint_external_int message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param target_system System ID + * @param target_component Component ID + * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: lat, bit 2: lon, bit 3: alt, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in WGS84, not AMSL + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param ax X acceleration in NED frame in meter / s^2 + * @param ay Y acceleration in NED frame in meter / s^2 + * @param az Z acceleration in NED frame in meter / s^2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_setpoint_external_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint16_t ignore_mask,int32_t lat_int,int32_t lon_int,float alt,float vx,float vy,float vz,float ax,float ay,float az) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, ax); + _mav_put_float(buf, 32, ay); + _mav_put_float(buf, 36, az); + _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_uint8_t(buf, 42, target_system); + _mav_put_uint8_t(buf, 43, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN); +#else + mavlink_global_position_setpoint_external_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ax = ax; + packet.ay = ay; + packet.az = az; + packet.ignore_mask = ignore_mask; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN); +#endif +} + +/** + * @brief Encode a global_position_setpoint_external_int struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param global_position_setpoint_external_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_setpoint_external_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_setpoint_external_int_t* global_position_setpoint_external_int) +{ + return mavlink_msg_global_position_setpoint_external_int_pack(system_id, component_id, msg, global_position_setpoint_external_int->time_boot_ms, global_position_setpoint_external_int->target_system, global_position_setpoint_external_int->target_component, global_position_setpoint_external_int->ignore_mask, global_position_setpoint_external_int->lat_int, global_position_setpoint_external_int->lon_int, global_position_setpoint_external_int->alt, global_position_setpoint_external_int->vx, global_position_setpoint_external_int->vy, global_position_setpoint_external_int->vz, global_position_setpoint_external_int->ax, global_position_setpoint_external_int->ay, global_position_setpoint_external_int->az); +} + +/** + * @brief Encode a global_position_setpoint_external_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_position_setpoint_external_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_setpoint_external_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_setpoint_external_int_t* global_position_setpoint_external_int) +{ + return mavlink_msg_global_position_setpoint_external_int_pack_chan(system_id, component_id, chan, msg, global_position_setpoint_external_int->time_boot_ms, global_position_setpoint_external_int->target_system, global_position_setpoint_external_int->target_component, global_position_setpoint_external_int->ignore_mask, global_position_setpoint_external_int->lat_int, global_position_setpoint_external_int->lon_int, global_position_setpoint_external_int->alt, global_position_setpoint_external_int->vx, global_position_setpoint_external_int->vy, global_position_setpoint_external_int->vz, global_position_setpoint_external_int->ax, global_position_setpoint_external_int->ay, global_position_setpoint_external_int->az); +} + +/** + * @brief Send a global_position_setpoint_external_int message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param target_system System ID + * @param target_component Component ID + * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: lat, bit 2: lon, bit 3: alt, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @param lat_int X Position in WGS84 frame in 1e7 * meters + * @param lon_int Y Position in WGS84 frame in 1e7 * meters + * @param alt Altitude in WGS84, not AMSL + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param ax X acceleration in NED frame in meter / s^2 + * @param ay Y acceleration in NED frame in meter / s^2 + * @param az Z acceleration in NED frame in meter / s^2 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_global_position_setpoint_external_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint16_t ignore_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float ax, float ay, float az) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, ax); + _mav_put_float(buf, 32, ay); + _mav_put_float(buf, 36, az); + _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_uint8_t(buf, 42, target_system); + _mav_put_uint8_t(buf, 43, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN); +#endif +#else + mavlink_global_position_setpoint_external_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ax = ax; + packet.ay = ay; + packet.az = az; + packet.ignore_mask = ignore_mask; + packet.target_system = target_system; + packet.target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_position_setpoint_external_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint16_t ignore_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float ax, float ay, float az) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, ax); + _mav_put_float(buf, 32, ay); + _mav_put_float(buf, 36, az); + _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_uint8_t(buf, 42, target_system); + _mav_put_uint8_t(buf, 43, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN); +#endif +#else + mavlink_global_position_setpoint_external_int_t *packet = (mavlink_global_position_setpoint_external_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->lat_int = lat_int; + packet->lon_int = lon_int; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->ax = ax; + packet->ay = ay; + packet->az = az; + packet->ignore_mask = ignore_mask; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GLOBAL_POSITION_SETPOINT_EXTERNAL_INT UNPACKING + + +/** + * @brief Get field time_boot_ms from global_position_setpoint_external_int message + * + * @return Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + */ +static inline uint32_t mavlink_msg_global_position_setpoint_external_int_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field target_system from global_position_setpoint_external_int message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_global_position_setpoint_external_int_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field target_component from global_position_setpoint_external_int message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_global_position_setpoint_external_int_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 43); +} + +/** + * @brief Get field ignore_mask from global_position_setpoint_external_int message + * + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: lat, bit 2: lon, bit 3: alt, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + */ +static inline uint16_t mavlink_msg_global_position_setpoint_external_int_get_ignore_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 40); +} + +/** + * @brief Get field lat_int from global_position_setpoint_external_int message + * + * @return X Position in WGS84 frame in 1e7 * meters + */ +static inline int32_t mavlink_msg_global_position_setpoint_external_int_get_lat_int(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field lon_int from global_position_setpoint_external_int message + * + * @return Y Position in WGS84 frame in 1e7 * meters + */ +static inline int32_t mavlink_msg_global_position_setpoint_external_int_get_lon_int(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field alt from global_position_setpoint_external_int message + * + * @return Altitude in WGS84, not AMSL + */ +static inline float mavlink_msg_global_position_setpoint_external_int_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field vx from global_position_setpoint_external_int message + * + * @return X velocity in NED frame in meter / s + */ +static inline float mavlink_msg_global_position_setpoint_external_int_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vy from global_position_setpoint_external_int message + * + * @return Y velocity in NED frame in meter / s + */ +static inline float mavlink_msg_global_position_setpoint_external_int_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vz from global_position_setpoint_external_int message + * + * @return Z velocity in NED frame in meter / s + */ +static inline float mavlink_msg_global_position_setpoint_external_int_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field ax from global_position_setpoint_external_int message + * + * @return X acceleration in NED frame in meter / s^2 + */ +static inline float mavlink_msg_global_position_setpoint_external_int_get_ax(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field ay from global_position_setpoint_external_int message + * + * @return Y acceleration in NED frame in meter / s^2 + */ +static inline float mavlink_msg_global_position_setpoint_external_int_get_ay(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field az from global_position_setpoint_external_int message + * + * @return Z acceleration in NED frame in meter / s^2 + */ +static inline float mavlink_msg_global_position_setpoint_external_int_get_az(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Decode a global_position_setpoint_external_int message into a struct + * + * @param msg The message to decode + * @param global_position_setpoint_external_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_global_position_setpoint_external_int_decode(const mavlink_message_t* msg, mavlink_global_position_setpoint_external_int_t* global_position_setpoint_external_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + global_position_setpoint_external_int->time_boot_ms = mavlink_msg_global_position_setpoint_external_int_get_time_boot_ms(msg); + global_position_setpoint_external_int->lat_int = mavlink_msg_global_position_setpoint_external_int_get_lat_int(msg); + global_position_setpoint_external_int->lon_int = mavlink_msg_global_position_setpoint_external_int_get_lon_int(msg); + global_position_setpoint_external_int->alt = mavlink_msg_global_position_setpoint_external_int_get_alt(msg); + global_position_setpoint_external_int->vx = mavlink_msg_global_position_setpoint_external_int_get_vx(msg); + global_position_setpoint_external_int->vy = mavlink_msg_global_position_setpoint_external_int_get_vy(msg); + global_position_setpoint_external_int->vz = mavlink_msg_global_position_setpoint_external_int_get_vz(msg); + global_position_setpoint_external_int->ax = mavlink_msg_global_position_setpoint_external_int_get_ax(msg); + global_position_setpoint_external_int->ay = mavlink_msg_global_position_setpoint_external_int_get_ay(msg); + global_position_setpoint_external_int->az = mavlink_msg_global_position_setpoint_external_int_get_az(msg); + global_position_setpoint_external_int->ignore_mask = mavlink_msg_global_position_setpoint_external_int_get_ignore_mask(msg); + global_position_setpoint_external_int->target_system = mavlink_msg_global_position_setpoint_external_int_get_target_system(msg); + global_position_setpoint_external_int->target_component = mavlink_msg_global_position_setpoint_external_int_get_target_component(msg); +#else + memcpy(global_position_setpoint_external_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h index 902e381ca..733886b9a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h @@ -7,7 +7,7 @@ typedef struct __mavlink_heartbeat_t uint32_t custom_mode; ///< A bitfield for use for autopilot-specific flags. uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) uint8_t autopilot; ///< Autopilot type / class. defined in MAV_AUTOPILOT ENUM - uint8_t base_mode; ///< System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h + uint8_t base_mode; ///< System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h uint8_t system_status; ///< System status flag, see MAV_STATE ENUM uint8_t mavlink_version; ///< MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version } mavlink_heartbeat_t; @@ -41,7 +41,7 @@ typedef struct __mavlink_heartbeat_t * * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - * @param base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h + * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h * @param custom_mode A bitfield for use for autopilot-specific flags. * @param system_status System status flag, see MAV_STATE ENUM * @return length of the message in bytes (excluding serial stream start sign) @@ -87,7 +87,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com * @param msg The MAVLink message to compress the data into * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - * @param base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h + * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h * @param custom_mode A bitfield for use for autopilot-specific flags. * @param system_status System status flag, see MAV_STATE ENUM * @return length of the message in bytes (excluding serial stream start sign) @@ -159,7 +159,7 @@ static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint * * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - * @param base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h + * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h * @param custom_mode A bitfield for use for autopilot-specific flags. * @param system_status System status flag, see MAV_STATE ENUM */ @@ -268,7 +268,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_ /** * @brief Get field base_mode from heartbeat message * - * @return System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h + * @return System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h */ static inline uint8_t mavlink_msg_heartbeat_get_base_mode(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_ned_position_setpoint_external.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_ned_position_setpoint_external.h new file mode 100644 index 000000000..6e60578ce --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_ned_position_setpoint_external.h @@ -0,0 +1,521 @@ +// MESSAGE LOCAL_NED_POSITION_SETPOINT_EXTERNAL PACKING + +#define MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL 83 + +typedef struct __mavlink_local_ned_position_setpoint_external_t +{ + uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot + float x; ///< X Position in NED frame in meters + float y; ///< Y Position in NED frame in meters + float z; ///< Z Position in NED frame in meters (note, altitude is negative in NED) + float vx; ///< X velocity in NED frame in meter / s + float vy; ///< Y velocity in NED frame in meter / s + float vz; ///< Z velocity in NED frame in meter / s + float ax; ///< X acceleration in NED frame in meter / s^2 + float ay; ///< Y acceleration in NED frame in meter / s^2 + float az; ///< Z acceleration in NED frame in meter / s^2 + uint16_t ignore_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t coordinate_frame; ///< Valid options are: MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_OFFSET_NED = 5, MAV_FRAME_BODY_NED = 6, MAV_FRAME_BODY_OFFSET_NED = 7 +} mavlink_local_ned_position_setpoint_external_t; + +#define MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN 45 +#define MAVLINK_MSG_ID_83_LEN 45 + +#define MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_CRC 121 +#define MAVLINK_MSG_ID_83_CRC 121 + + + +#define MAVLINK_MESSAGE_INFO_LOCAL_NED_POSITION_SETPOINT_EXTERNAL { \ + "LOCAL_NED_POSITION_SETPOINT_EXTERNAL", \ + 14, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_local_ned_position_setpoint_external_t, time_boot_ms) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_ned_position_setpoint_external_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_ned_position_setpoint_external_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_ned_position_setpoint_external_t, z) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_ned_position_setpoint_external_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_ned_position_setpoint_external_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_ned_position_setpoint_external_t, vz) }, \ + { "ax", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_local_ned_position_setpoint_external_t, ax) }, \ + { "ay", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_local_ned_position_setpoint_external_t, ay) }, \ + { "az", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_local_ned_position_setpoint_external_t, az) }, \ + { "ignore_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_local_ned_position_setpoint_external_t, ignore_mask) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_local_ned_position_setpoint_external_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_local_ned_position_setpoint_external_t, target_component) }, \ + { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_local_ned_position_setpoint_external_t, coordinate_frame) }, \ + } \ +} + + +/** + * @brief Pack a local_ned_position_setpoint_external message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_OFFSET_NED = 5, MAV_FRAME_BODY_NED = 6, MAV_FRAME_BODY_OFFSET_NED = 7 + * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param ax X acceleration in NED frame in meter / s^2 + * @param ay Y acceleration in NED frame in meter / s^2 + * @param az Z acceleration in NED frame in meter / s^2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t ignore_mask, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, ax); + _mav_put_float(buf, 32, ay); + _mav_put_float(buf, 36, az); + _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_uint8_t(buf, 42, target_system); + _mav_put_uint8_t(buf, 43, target_component); + _mav_put_uint8_t(buf, 44, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN); +#else + mavlink_local_ned_position_setpoint_external_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ax = ax; + packet.ay = ay; + packet.az = az; + packet.ignore_mask = ignore_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN); +#endif +} + +/** + * @brief Pack a local_ned_position_setpoint_external message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_OFFSET_NED = 5, MAV_FRAME_BODY_NED = 6, MAV_FRAME_BODY_OFFSET_NED = 7 + * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param ax X acceleration in NED frame in meter / s^2 + * @param ay Y acceleration in NED frame in meter / s^2 + * @param az Z acceleration in NED frame in meter / s^2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t coordinate_frame,uint16_t ignore_mask,float x,float y,float z,float vx,float vy,float vz,float ax,float ay,float az) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, ax); + _mav_put_float(buf, 32, ay); + _mav_put_float(buf, 36, az); + _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_uint8_t(buf, 42, target_system); + _mav_put_uint8_t(buf, 43, target_component); + _mav_put_uint8_t(buf, 44, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN); +#else + mavlink_local_ned_position_setpoint_external_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ax = ax; + packet.ay = ay; + packet.az = az; + packet.ignore_mask = ignore_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN); +#endif +} + +/** + * @brief Encode a local_ned_position_setpoint_external struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param local_ned_position_setpoint_external C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_ned_position_setpoint_external_t* local_ned_position_setpoint_external) +{ + return mavlink_msg_local_ned_position_setpoint_external_pack(system_id, component_id, msg, local_ned_position_setpoint_external->time_boot_ms, local_ned_position_setpoint_external->target_system, local_ned_position_setpoint_external->target_component, local_ned_position_setpoint_external->coordinate_frame, local_ned_position_setpoint_external->ignore_mask, local_ned_position_setpoint_external->x, local_ned_position_setpoint_external->y, local_ned_position_setpoint_external->z, local_ned_position_setpoint_external->vx, local_ned_position_setpoint_external->vy, local_ned_position_setpoint_external->vz, local_ned_position_setpoint_external->ax, local_ned_position_setpoint_external->ay, local_ned_position_setpoint_external->az); +} + +/** + * @brief Encode a local_ned_position_setpoint_external struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_ned_position_setpoint_external C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_ned_position_setpoint_external_t* local_ned_position_setpoint_external) +{ + return mavlink_msg_local_ned_position_setpoint_external_pack_chan(system_id, component_id, chan, msg, local_ned_position_setpoint_external->time_boot_ms, local_ned_position_setpoint_external->target_system, local_ned_position_setpoint_external->target_component, local_ned_position_setpoint_external->coordinate_frame, local_ned_position_setpoint_external->ignore_mask, local_ned_position_setpoint_external->x, local_ned_position_setpoint_external->y, local_ned_position_setpoint_external->z, local_ned_position_setpoint_external->vx, local_ned_position_setpoint_external->vy, local_ned_position_setpoint_external->vz, local_ned_position_setpoint_external->ax, local_ned_position_setpoint_external->ay, local_ned_position_setpoint_external->az); +} + +/** + * @brief Send a local_ned_position_setpoint_external message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp in milliseconds since system boot + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_OFFSET_NED = 5, MAV_FRAME_BODY_NED = 6, MAV_FRAME_BODY_OFFSET_NED = 7 + * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @param x X Position in NED frame in meters + * @param y Y Position in NED frame in meters + * @param z Z Position in NED frame in meters (note, altitude is negative in NED) + * @param vx X velocity in NED frame in meter / s + * @param vy Y velocity in NED frame in meter / s + * @param vz Z velocity in NED frame in meter / s + * @param ax X acceleration in NED frame in meter / s^2 + * @param ay Y acceleration in NED frame in meter / s^2 + * @param az Z acceleration in NED frame in meter / s^2 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_local_ned_position_setpoint_external_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t ignore_mask, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, ax); + _mav_put_float(buf, 32, ay); + _mav_put_float(buf, 36, az); + _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_uint8_t(buf, 42, target_system); + _mav_put_uint8_t(buf, 43, target_component); + _mav_put_uint8_t(buf, 44, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, buf, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, buf, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN); +#endif +#else + mavlink_local_ned_position_setpoint_external_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ax = ax; + packet.ay = ay; + packet.az = az; + packet.ignore_mask = ignore_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_ned_position_setpoint_external_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t ignore_mask, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, ax); + _mav_put_float(buf, 32, ay); + _mav_put_float(buf, 36, az); + _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_uint8_t(buf, 42, target_system); + _mav_put_uint8_t(buf, 43, target_component); + _mav_put_uint8_t(buf, 44, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, buf, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, buf, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN); +#endif +#else + mavlink_local_ned_position_setpoint_external_t *packet = (mavlink_local_ned_position_setpoint_external_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->ax = ax; + packet->ay = ay; + packet->az = az; + packet->ignore_mask = ignore_mask; + packet->target_system = target_system; + packet->target_component = target_component; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, (const char *)packet, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, (const char *)packet, MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE LOCAL_NED_POSITION_SETPOINT_EXTERNAL UNPACKING + + +/** + * @brief Get field time_boot_ms from local_ned_position_setpoint_external message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_local_ned_position_setpoint_external_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field target_system from local_ned_position_setpoint_external message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_local_ned_position_setpoint_external_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field target_component from local_ned_position_setpoint_external message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_local_ned_position_setpoint_external_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 43); +} + +/** + * @brief Get field coordinate_frame from local_ned_position_setpoint_external message + * + * @return Valid options are: MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_OFFSET_NED = 5, MAV_FRAME_BODY_NED = 6, MAV_FRAME_BODY_OFFSET_NED = 7 + */ +static inline uint8_t mavlink_msg_local_ned_position_setpoint_external_get_coordinate_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 44); +} + +/** + * @brief Get field ignore_mask from local_ned_position_setpoint_external message + * + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + */ +static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_get_ignore_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 40); +} + +/** + * @brief Get field x from local_ned_position_setpoint_external message + * + * @return X Position in NED frame in meters + */ +static inline float mavlink_msg_local_ned_position_setpoint_external_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field y from local_ned_position_setpoint_external message + * + * @return Y Position in NED frame in meters + */ +static inline float mavlink_msg_local_ned_position_setpoint_external_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field z from local_ned_position_setpoint_external message + * + * @return Z Position in NED frame in meters (note, altitude is negative in NED) + */ +static inline float mavlink_msg_local_ned_position_setpoint_external_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field vx from local_ned_position_setpoint_external message + * + * @return X velocity in NED frame in meter / s + */ +static inline float mavlink_msg_local_ned_position_setpoint_external_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vy from local_ned_position_setpoint_external message + * + * @return Y velocity in NED frame in meter / s + */ +static inline float mavlink_msg_local_ned_position_setpoint_external_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vz from local_ned_position_setpoint_external message + * + * @return Z velocity in NED frame in meter / s + */ +static inline float mavlink_msg_local_ned_position_setpoint_external_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field ax from local_ned_position_setpoint_external message + * + * @return X acceleration in NED frame in meter / s^2 + */ +static inline float mavlink_msg_local_ned_position_setpoint_external_get_ax(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field ay from local_ned_position_setpoint_external message + * + * @return Y acceleration in NED frame in meter / s^2 + */ +static inline float mavlink_msg_local_ned_position_setpoint_external_get_ay(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field az from local_ned_position_setpoint_external message + * + * @return Z acceleration in NED frame in meter / s^2 + */ +static inline float mavlink_msg_local_ned_position_setpoint_external_get_az(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Decode a local_ned_position_setpoint_external message into a struct + * + * @param msg The message to decode + * @param local_ned_position_setpoint_external C-struct to decode the message contents into + */ +static inline void mavlink_msg_local_ned_position_setpoint_external_decode(const mavlink_message_t* msg, mavlink_local_ned_position_setpoint_external_t* local_ned_position_setpoint_external) +{ +#if MAVLINK_NEED_BYTE_SWAP + local_ned_position_setpoint_external->time_boot_ms = mavlink_msg_local_ned_position_setpoint_external_get_time_boot_ms(msg); + local_ned_position_setpoint_external->x = mavlink_msg_local_ned_position_setpoint_external_get_x(msg); + local_ned_position_setpoint_external->y = mavlink_msg_local_ned_position_setpoint_external_get_y(msg); + local_ned_position_setpoint_external->z = mavlink_msg_local_ned_position_setpoint_external_get_z(msg); + local_ned_position_setpoint_external->vx = mavlink_msg_local_ned_position_setpoint_external_get_vx(msg); + local_ned_position_setpoint_external->vy = mavlink_msg_local_ned_position_setpoint_external_get_vy(msg); + local_ned_position_setpoint_external->vz = mavlink_msg_local_ned_position_setpoint_external_get_vz(msg); + local_ned_position_setpoint_external->ax = mavlink_msg_local_ned_position_setpoint_external_get_ax(msg); + local_ned_position_setpoint_external->ay = mavlink_msg_local_ned_position_setpoint_external_get_ay(msg); + local_ned_position_setpoint_external->az = mavlink_msg_local_ned_position_setpoint_external_get_az(msg); + local_ned_position_setpoint_external->ignore_mask = mavlink_msg_local_ned_position_setpoint_external_get_ignore_mask(msg); + local_ned_position_setpoint_external->target_system = mavlink_msg_local_ned_position_setpoint_external_get_target_system(msg); + local_ned_position_setpoint_external->target_component = mavlink_msg_local_ned_position_setpoint_external_get_target_component(msg); + local_ned_position_setpoint_external->coordinate_frame = mavlink_msg_local_ned_position_setpoint_external_get_coordinate_frame(msg); +#else + memcpy(local_ned_position_setpoint_external, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h index de23fcb2a..ccfe74f08 100644 --- a/mavlink/include/mavlink/v1.0/common/testsuite.h +++ b/mavlink/include/mavlink/v1.0/common/testsuite.h @@ -3367,6 +3367,201 @@ static void mavlink_test_manual_setpoint(uint8_t system_id, uint8_t component_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_attitude_setpoint_external(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_attitude_setpoint_external_t packet_in = { + 963497464, + }{ 45.0, 46.0, 47.0, 48.0 }, + }157.0, + }185.0, + }213.0, + }241.0, + }113, + }180, + }247, + }; + mavlink_attitude_setpoint_external_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.body_roll_rate = packet_in.body_roll_rate; + packet1.body_pitch_rate = packet_in.body_pitch_rate; + packet1.body_yaw_rate = packet_in.body_yaw_rate; + packet1.thrust = packet_in.thrust; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.ignore_mask = packet_in.ignore_mask; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_attitude_setpoint_external_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_attitude_setpoint_external_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_attitude_setpoint_external_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.ignore_mask , packet1.q , packet1.body_roll_rate , packet1.body_pitch_rate , packet1.body_yaw_rate , packet1.thrust ); + mavlink_msg_attitude_setpoint_external_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_attitude_setpoint_external_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.ignore_mask , packet1.q , packet1.body_roll_rate , packet1.body_pitch_rate , packet1.body_yaw_rate , packet1.thrust ); + mavlink_msg_attitude_setpoint_external_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ @@ -68,7 +72,7 @@ enum MAV_CMD MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Custom mode - this is system specific, please refer to the individual autopilot specifications for details.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ @@ -86,6 +90,7 @@ enum MAV_CMD MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ @@ -97,7 +102,7 @@ enum MAV_CMD MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ MAV_CMD_ENUM_END=501, /* | */ -}; +} MAV_CMD; #endif #include "../common/common.h" diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/version.h b/mavlink/include/mavlink/v1.0/matrixpilot/version.h index 50cbda6c7..40273061d 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/version.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Sun Apr 13 09:38:32 2014" +#define MAVLINK_BUILD_DATE "Mon Jun 23 10:13:29 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/mavlink_helpers.h b/mavlink/include/mavlink/v1.0/mavlink_helpers.h index 96672f847..1639a830b 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_helpers.h +++ b/mavlink/include/mavlink/v1.0/mavlink_helpers.h @@ -73,7 +73,6 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui #endif { // This code part is the same for all messages; - uint16_t checksum; msg->magic = MAVLINK_STX; msg->len = length; msg->sysid = system_id; @@ -81,12 +80,13 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui // One sequence number per component msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN); + msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN); + crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len); #if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); + crc_accumulate(crc_extra, &msg->checksum); #endif - mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); - mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); + mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF); + mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8); return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -133,7 +133,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint buf[4] = mavlink_system.compid; buf[5] = msgid; status->current_tx_seq++; - checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); crc_accumulate_buffer(&checksum, packet, length); #if MAVLINK_CRC_EXTRA crc_accumulate(crc_extra, &checksum); @@ -158,6 +158,7 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m ck[0] = (uint8_t)(msg->checksum & 0xFF); ck[1] = (uint8_t)(msg->checksum >> 8); + // XXX use the right sequence here MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); @@ -172,7 +173,13 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m */ MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) { - memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len); + memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); + + uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); + + ck[0] = (uint8_t)(msg->checksum & 0xFF); + ck[1] = (uint8_t)(msg->checksum >> 8); + return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; } diff --git a/mavlink/include/mavlink/v1.0/mavlink_types.h b/mavlink/include/mavlink/v1.0/mavlink_types.h index 4019c619e..43658e629 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_types.h +++ b/mavlink/include/mavlink/v1.0/mavlink_types.h @@ -28,6 +28,7 @@ #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) +#pragma pack(push, 1) typedef struct param_union { union { float param_float; @@ -62,13 +63,12 @@ typedef struct __mavlink_message { uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; } mavlink_message_t; - typedef struct __mavlink_extended_message { mavlink_message_t base_msg; int32_t extended_payload_len; ///< Length of extended payload if any uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; } mavlink_extended_message_t; - +#pragma pack(pop) typedef enum { MAVLINK_TYPE_CHAR = 0, diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h index a624a2579..6a55f592b 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h @@ -5,6 +5,10 @@ #ifndef PIXHAWK_H #define PIXHAWK_H +#ifndef MAVLINK_H + #error Wrong include order: PIXHAWK.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. +#endif + #ifdef __cplusplus extern "C" { #endif @@ -12,15 +16,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 0, 0, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 39, 45, 44, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 0, 0, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 108, 86, 95, 224, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 0, 0, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 212, 121, 172, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 108, 86, 95, 224, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 0, 0, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_POSITION_CONTROL_OFFSET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_ATTITUDE_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_POSITION_CONTROL_OFFSET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -33,19 +37,19 @@ extern "C" { /** @brief Content Types for data transmission handshake */ #ifndef HAVE_ENUM_DATA_TYPES #define HAVE_ENUM_DATA_TYPES -enum DATA_TYPES +typedef enum DATA_TYPES { DATA_TYPE_JPEG_IMAGE=1, /* | */ DATA_TYPE_RAW_IMAGE=2, /* | */ DATA_TYPE_KINECT=3, /* | */ DATA_TYPES_ENUM_END=4, /* | */ -}; +} DATA_TYPES; #endif /** @brief */ #ifndef HAVE_ENUM_MAV_CMD #define HAVE_ENUM_MAV_CMD -enum MAV_CMD +typedef enum MAV_CMD { MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ @@ -63,7 +67,7 @@ enum MAV_CMD MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Custom mode - this is system specific, please refer to the individual autopilot specifications for details.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ @@ -81,6 +85,7 @@ enum MAV_CMD MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ @@ -95,7 +100,7 @@ enum MAV_CMD MAV_CMD_DO_START_SEARCH=10001, /* Starts a search |1 to arm, 0 to disarm| */ MAV_CMD_DO_FINISH_SEARCH=10002, /* Starts a search |1 to arm, 0 to disarm| */ MAV_CMD_NAV_SWEEP=10003, /* Starts a search |1 to arm, 0 to disarm| */ -}; +} MAV_CMD; #endif #include "../common/common.h" diff --git a/mavlink/include/mavlink/v1.0/pixhawk/version.h b/mavlink/include/mavlink/v1.0/pixhawk/version.h index 6610a894c..6a639918e 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/version.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Sun Apr 13 09:38:46 2014" +#define MAVLINK_BUILD_DATE "Mon Jun 23 10:13:53 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h b/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h index 2eab86354..4fb651853 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h @@ -5,6 +5,10 @@ #ifndef SENSESOAR_H #define SENSESOAR_H +#ifndef MAVLINK_H + #error Wrong include order: SENSESOAR.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. +#endif + #ifdef __cplusplus extern "C" { #endif @@ -12,15 +16,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 39, 45, 44, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 212, 121, 172, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_POSITION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_ATTITUDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_WIND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_BIAS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_QFF, MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP, MAVLINK_MESSAGE_INFO_FILT_ROT_VEL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LLC_OUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PM_ELEC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_Stat, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_ATTITUDE_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_POSITION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_ATTITUDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_WIND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_BIAS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_QFF, MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP, MAVLINK_MESSAGE_INFO_FILT_ROT_VEL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LLC_OUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PM_ELEC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_Stat, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -33,13 +37,13 @@ extern "C" { /** @brief Different flight modes */ #ifndef HAVE_ENUM_SENSESOAR_MODE #define HAVE_ENUM_SENSESOAR_MODE -enum SENSESOAR_MODE +typedef enum SENSESOAR_MODE { SENSESOAR_MODE_GLIDING=1, /* Gliding mode with motors off | */ SENSESOAR_MODE_AUTONOMOUS=2, /* Autonomous flight | */ SENSESOAR_MODE_MANUAL=3, /* RC controlled | */ SENSESOAR_MODE_ENUM_END=4, /* | */ -}; +} SENSESOAR_MODE; #endif #include "../common/common.h" diff --git a/mavlink/include/mavlink/v1.0/sensesoar/version.h b/mavlink/include/mavlink/v1.0/sensesoar/version.h index 78d562803..de8a60750 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/version.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Sun Apr 13 09:39:00 2014" +#define MAVLINK_BUILD_DATE "Mon Jun 23 10:13:41 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 -- cgit v1.2.3 From cfdbf2c5e914c6264d9158470d4f98c84a483f68 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Jun 2014 11:15:27 +0200 Subject: perfcounter: write time unit for all fields --- src/modules/systemlib/perf_counter.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 22182e39e..d6d8284d2 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -301,7 +301,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - dprintf(fd, "%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n", + dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n", handle->name, pce->event_count, pce->time_total, @@ -314,7 +314,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_INTERVAL: { struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - dprintf(fd, "%s: %llu events, %llu avg, min %lluus max %lluus\n", + dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n", handle->name, pci->event_count, (pci->time_last - pci->time_first) / pci->event_count, -- cgit v1.2.3 From 88adeee6695a2160f9eac130d3923129728dd008 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Jun 2014 13:35:14 +0200 Subject: define force setpoint uorb topic --- src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/vehicle_force_setpoint.h | 65 ++++++++++++++++++++++++ 2 files changed, 68 insertions(+) create mode 100644 src/modules/uORB/topics/vehicle_force_setpoint.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e..d45314222 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -199,3 +199,6 @@ ORB_DEFINE(encoders, struct encoders_s); #include "topics/estimator_status.h" ORB_DEFINE(estimator_status, struct estimator_status_report); + +#include "topics/vehicle_force_setpoint.h" +ORB_DEFINE(vehicle_force_setpoint, struct vehicle_force_setpoint_s); diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/modules/uORB/topics/vehicle_force_setpoint.h new file mode 100644 index 000000000..c7b5f558e --- /dev/null +++ b/src/modules/uORB/topics/vehicle_force_setpoint.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_local_force_setpoint.h + * @author Thomas Gubler + * Definition of force (NED) setpoint uORB topic. Typically this can be used + * by a position control app togeth with an attitude control app. + */ + +#ifndef TOPIC_VEHICLE_FORCE_SETPOINT_H_ +#define TOPIC_VEHICLE_FORCE_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_force_setpoint_s { + float x; /**< in N NED */ + float y; /**< in N NED */ + float z; /**< in N NED */ +}; /**< Desired force in NED frame */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_force_setpoint); + +#endif -- cgit v1.2.3 From d58a992e911114383a44327eb4478193824b580d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Jun 2014 13:44:36 +0200 Subject: Hotfix: Improve PX4IO monitor command --- src/drivers/px4io/px4io.cpp | 38 ++++++++++++++++++++---------------- src/drivers/px4io/px4io_uploader.cpp | 6 +++--- 2 files changed, 24 insertions(+), 20 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 972f45148..992ab9623 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -197,8 +197,10 @@ public: * Print IO status. * * Print all relevant IO status information + * + * @param extended_status Shows more verbose information (in particular RC config) */ - void print_status(); + void print_status(bool extended_status); /** * Fetch and print debug console output. @@ -1850,7 +1852,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } void -PX4IO::print_status() +PX4IO::print_status(bool extended_status) { /* basic configuration */ printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n", @@ -2013,19 +2015,21 @@ PX4IO::print_status() printf("\n"); } - for (unsigned i = 0; i < _max_rc_input; i++) { - unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; - uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); - printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", - i, - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), - options, - ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), - ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + if (extended_status) { + for (unsigned i = 0; i < _max_rc_input; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; + uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); + printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", + i, + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), + options, + ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), + ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + } } printf("failsafe"); @@ -2853,7 +2857,7 @@ monitor(void) if (g_dev != nullptr) { printf("\033[2J\033[H"); /* move cursor home and clear screen */ - (void)g_dev->print_status(); + (void)g_dev->print_status(false); (void)g_dev->print_debug(); printf("\n\n\n[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); @@ -3119,7 +3123,7 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { printf("[px4io] loaded\n"); - g_dev->print_status(); + g_dev->print_status(true); exit(0); } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 28ec62356..7b6361a7c 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -240,9 +240,9 @@ PX4IO_Uploader::upload(const char *filenames[]) close(_io_fd); _io_fd = -1; - // sleep for enough time for the IO chip to boot. This makes - // forceupdate more reliably startup IO again after update - up_udelay(100*1000); + // sleep for enough time for the IO chip to boot. This makes + // forceupdate more reliably startup IO again after update + up_udelay(100*1000); return ret; } -- cgit v1.2.3 From 87857cdd48d43a28c3b8ed1f1fe500ad28a93bbc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Jun 2014 13:45:20 +0200 Subject: Hotfix: Fix message name typo --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index aff1aa929..fed2dfb0d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1625,7 +1625,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), + new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), -- cgit v1.2.3 From a9653fa10db3884d3d17ee33f80f23aa2e3ef842 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Jun 2014 13:51:05 +0200 Subject: Hotfix: Only orb_copy items in mavlink app if the timestamp changed --- src/modules/mavlink/mavlink_orb_subscription.cpp | 26 ++++++++++++++---------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 901fa8f05..3807323c2 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -44,6 +44,8 @@ #include #include +#include + #include "mavlink_orb_subscription.h" MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : @@ -77,21 +79,23 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data) time_topic = 0; } - if (orb_copy(_topic, _fd, data)) { - /* error copying topic data */ - memset(data, 0, _topic->o_size); - return false; + if (time_topic != *time) { - } else { - /* data copied successfully */ - _published = true; - if (time_topic != *time) { - *time = time_topic; - return true; + if (orb_copy(_topic, _fd, data)) { + /* error copying topic data */ + memset(data, 0, _topic->o_size); + //warnx("err copy, fd: %d, obj: %s, size: %d", _fd, _topic->o_name, _topic->o_size); + return false; } else { - return false; + /* data copied successfully */ + _published = true; + *time = time_topic; + return true; } + + } else { + return false; } } -- cgit v1.2.3 From bf5061aa21872c98576d46aee894e670ce0c52a0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Jun 2014 13:52:09 +0200 Subject: Fix error reporting in stream config, report if a stream was not found at all in stream list and return error --- src/modules/mavlink/mavlink_main.cpp | 36 ++++++++++++++++++++---------------- 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 046f45bd9..a9b8323f3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1555,32 +1555,36 @@ Mavlink::configure_stream(const char *stream_name, const float rate) /* delete stream */ LL_DELETE(_streams, stream); delete stream; + warnx("deleted stream %s", stream->get_name()); } return OK; } } - if (interval > 0) { - /* search for stream with specified name in supported streams list */ - for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + if (interval == 0) { + /* stream was not active and is requested to be disabled, do nothing */ + return OK; + } - if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { - /* create new instance */ - stream = streams_list[i]->new_instance(); - stream->set_channel(get_channel()); - stream->set_interval(interval); - stream->subscribe(this); - LL_APPEND(_streams, stream); - return OK; - } - } + /* search for stream with specified name in supported streams list */ + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { - } else { - /* stream not found, nothing to disable */ - return OK; + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { + /* create new instance */ + stream = streams_list[i]->new_instance(); + stream->set_channel(get_channel()); + stream->set_interval(interval); + stream->subscribe(this); + LL_APPEND(_streams, stream); + + return OK; + } } + /* if we reach here, the stream list does not contain the stream */ + warnx("stream %s not found", stream_name); + return ERROR; } -- cgit v1.2.3 From 68442e31ac6970be91592282c9b70ebc76fa142d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Jun 2014 14:56:48 +0200 Subject: Hotfix: Move channel count to right position --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fed2dfb0d..4433377c6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1275,6 +1275,7 @@ protected: // New message mavlink_msg_rc_channels_send(_channel, rc.timestamp_publication / 1000, + rc.channel_count, ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), @@ -1293,7 +1294,6 @@ protected: ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), - rc.channel_count, rc.rssi); } } -- cgit v1.2.3 From 179bace35aaa32a23ebd7a9cb99d54eae53690f5 Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 24 Jun 2014 07:47:16 -0400 Subject: Resets XY velocities when we can't estimate them --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 7 +++++++ .../position_estimator_inav/position_estimator_inav_params.c | 3 +++ .../position_estimator_inav/position_estimator_inav_params.h | 2 ++ 3 files changed, 12 insertions(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index f908d7a3b..5f132453b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -916,6 +916,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memcpy(x_est_prev, x_est, sizeof(x_est)); memcpy(y_est_prev, y_est, sizeof(y_est)); } + } else { + /* gradually reset xy velocity estimates */ + inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_reset_v); + inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_reset_v); } /* detect land */ @@ -931,6 +935,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed = false; landed_time = 0; } + /* reset xy velocity estimates when landed */ + x_est[1] = 0.0f; + y_est[1] = 0.0f; } else { if (alt_disp2 < land_disp2 && thrust < params.land_thr) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index d88419c25..b79e00274 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -46,6 +46,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_RESET_V, 0.2f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); @@ -65,6 +66,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); + h->w_xy_reset_v = param_find("INAV_W_XY_RESET_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); @@ -87,6 +89,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); param_get(h->w_xy_flow, &(p->w_xy_flow)); + param_get(h->w_xy_reset_v, &(p->w_xy_reset_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index df1cfaa71..9be13637d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -47,6 +47,7 @@ struct position_estimator_inav_params { float w_xy_gps_p; float w_xy_gps_v; float w_xy_flow; + float w_xy_reset_v; float w_gps_flow; float w_acc_bias; float flow_k; @@ -66,6 +67,7 @@ struct position_estimator_inav_param_handles { param_t w_xy_gps_p; param_t w_xy_gps_v; param_t w_xy_flow; + param_t w_xy_reset_v; param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; -- cgit v1.2.3 From 3ddb502d2e1b07705bfaa59c89528d53d0444303 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 14:14:13 +0200 Subject: force setpoint: fix comment --- src/modules/uORB/topics/vehicle_force_setpoint.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/modules/uORB/topics/vehicle_force_setpoint.h index c7b5f558e..cd948985f 100644 --- a/src/modules/uORB/topics/vehicle_force_setpoint.h +++ b/src/modules/uORB/topics/vehicle_force_setpoint.h @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,10 +32,10 @@ ****************************************************************************/ /** - * @file vehicle_local_force_setpoint.h + * @file vehicle_force_setpoint.h * @author Thomas Gubler * Definition of force (NED) setpoint uORB topic. Typically this can be used - * by a position control app togeth with an attitude control app. + * by a position control app together with an attitude control app. */ #ifndef TOPIC_VEHICLE_FORCE_SETPOINT_H_ -- cgit v1.2.3 From 52d539d3cdf56eb4145dbf29c407bb0a1a7eebe5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 17:39:48 +0200 Subject: extend integrator reset flags in uorb attitude setpoint topic to all axes --- src/modules/uORB/topics/vehicle_attitude_setpoint.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index d526a8ff2..8446e9c6e 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s { float thrust; /**< Thrust in Newton the power system should generate */ bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ + bool pitch_reset_integral; /**< Reset pitch integral part (navigation logic change) */ + bool yaw_reset_integral; /**< Reset yaw integral part (navigation logic change) */ }; -- cgit v1.2.3 From 9a911f7388e1a48630469fd2e875f00a119c829a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 17:40:56 +0200 Subject: fw att control: reset integrators when requested via attitude setpoint topic --- src/modules/fw_att_control/fw_att_control_main.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 178b590ae..3cd4ce928 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -707,14 +707,21 @@ FixedwingAttitudeControl::task_main() float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { + /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) + if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); - + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } } else { /* * Scale down roll and pitch as the setpoints are radians -- cgit v1.2.3 From c6cdcfc2638f8d994b3716d8739614c5377c4962 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 17:42:21 +0200 Subject: fw pos control: set integrator reset flags in attitude setpoint topic, set them to true when launchdetection is running (while waiting for launch) --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 116d3cc63..518116fa1 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -841,6 +841,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* current waypoint (the one currently heading for) */ math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); + /* Initialize attitude controller integrator reset flags to 0 */ + _att_sp.roll_reset_integral = false; + _att_sp.pitch_reset_integral = false; + _att_sp.yaw_reset_integral = false; /* previous waypoint */ math::Vector<2> prev_wp; @@ -1028,6 +1032,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); last_sent = hrt_absolute_time(); + /* Tell the attitude controller to stop integrating while we are + * waiting for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; } launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { -- cgit v1.2.3 From 32319722a60db2a43a2ac80a579eee09cb0a5dd0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Jun 2014 20:47:22 +0200 Subject: Retry fusion sooner on failures --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 9be48cfea..1a2151017 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -925,9 +925,9 @@ void AttPosEKF::FuseVelposNED() { // declare variables used by fault isolation logic - uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure - uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available - uint32_t hgtRetryTime = 5000; // height measurement retry time + uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure + uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available + uint32_t hgtRetryTime = 500; // height measurement retry time uint32_t horizRetryTime; // declare variables used to check measurement errors @@ -2587,6 +2587,7 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *state) for (unsigned i = 0; i < n_states; i++) { current_ekf_state.states[i] = states[i]; } + current_ekf_state.n_states = n_states; memcpy(state, ¤t_ekf_state, sizeof(*state)); } -- cgit v1.2.3 From 7f41ec52f15191f7a0bbc0ec32301c3135184840 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Jun 2014 20:48:18 +0200 Subject: estimator: Introduce debug level to allow high-res bench debugging - set with ekf_att_pos_estimator debug --- .../ekf_att_pos_estimator_main.cpp | 49 +++++++++++++++++++++- 1 file changed, 47 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index cb29b913c..5b0e7acf2 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -120,7 +120,7 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ int start(); @@ -136,9 +136,18 @@ public: /** * Enable logging. + * + * @param enable Set to true to enable logging, false to disable */ int enable_logging(bool enable); + /** + * Set debug level. + * + * @param debug Desired debug level - 0 to disable. + */ + int set_debuglevel(unsigned debug) { _debug = debug; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -210,6 +219,7 @@ private: bool _accel_valid; bool _mag_valid; bool _ekf_logging; ///< log EKF state + unsigned _debug; ///< debug level - default 0 int _mavlink_fd; @@ -377,6 +387,7 @@ FixedwingEstimator::FixedwingEstimator() : _accel_valid(false), _mag_valid(false), _ekf_logging(true), + _debug(0), _mavlink_fd(-1), _ekf(nullptr), _velocity_xy_filtered(0.0f), @@ -655,8 +666,26 @@ FixedwingEstimator::check_filter_state() rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); + if (_debug > 10) { + + if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { + warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s", + ((rep.timeout_flags & (1 << 0)) ? "OK" : "ERR"), + ((rep.timeout_flags & (1 << 1)) ? "OK" : "ERR"), + ((rep.timeout_flags & (1 << 2)) ? "OK" : "ERR"), + ((rep.timeout_flags & (1 << 3)) ? "OK" : "ERR")); + } + + if (rep.timeout_flags) { + warnx("timeout: %s%s%s", + ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), + ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), + ((rep.timeout_flags & (1 << 2)) ? "HGT " : "")); + } + } + // Copy all states or at least all that we can fit - unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); + unsigned ekf_n_states = ekf_report.n_states; unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; @@ -664,6 +693,10 @@ FixedwingEstimator::check_filter_state() rep.states[i] = ekf_report.states[i]; } + for (unsigned i = 0; i < rep.n_states; i++) { + rep.states[i] = ekf_report.states[i]; + } + if (_estimator_status_pub > 0) { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); } else { @@ -1683,6 +1716,18 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) } } + if (!strcmp(argv[1], "debug")) { + if (estimator::g_estimator) { + int debug = strtoul(argv[2], NULL, 10); + int ret = estimator::g_estimator->set_debuglevel(debug); + + exit(ret); + + } else { + errx(1, "not running"); + } + } + warnx("unrecognized command"); return 1; } -- cgit v1.2.3 From 819812e11271d7b78f4af2d5bbb7ac6e4be9e494 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 20:52:24 +0200 Subject: fw pos ctrl: move setting of attitude integral reset flag --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 518116fa1..fe5847682 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1032,12 +1032,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); last_sent = hrt_absolute_time(); - /* Tell the attitude controller to stop integrating while we are - * waiting for the launch */ - _att_sp.roll_reset_integral = true; - _att_sp.pitch_reset_integral = true; - _att_sp.yaw_reset_integral = true; } + + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + + /* Detect launch */ launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { launch_detected = true; -- cgit v1.2.3 From 4f560f729ec1f40427401119a0f17d9d0e9843f4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 20:53:06 +0200 Subject: fw pos ctrl: remove comments --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index fe5847682..0e065211e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1024,12 +1024,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ -// warnx("Launch detection running"); if(!launch_detected) { //do not do further checks once a launch was detected if (launchDetector.launchDetectionEnabled()) { static hrt_abstime last_sent = 0; if(hrt_absolute_time() - last_sent > 4e6) { -// warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); last_sent = hrt_absolute_time(); } -- cgit v1.2.3 From 571e139c24464902470901501c5bbfb618c1258c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Jun 2014 21:33:03 +0200 Subject: commander: Formatting and logic flow cleanup, no functional change --- src/modules/commander/state_machine_helper.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87309b238..1a5a52a02 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +34,9 @@ /** * @file state_machine_helper.cpp * State machine helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes */ #include @@ -133,15 +134,20 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (valid_transition) { // We have a good transition. Now perform any secondary validation. if (new_arming_state == ARMING_STATE_ARMED) { - // Fail transition if we need safety switch press // Allow if coming from in air restore // Allow if HIL_STATE_ON - if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); - } + if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && + status->hil_state == HIL_STATE_OFF) { + + // Fail transition if we need safety switch press + if (safety->safety_switch_available && !safety->safety_off) { - valid_transition = false; + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); + } + + valid_transition = false; + } } } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { -- cgit v1.2.3 From b1f223b468ab5ff73c6a39749dbaf43f2f46a90b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Jun 2014 22:22:56 +0200 Subject: commander: Default all leds to off --- src/modules/commander/commander_helper.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 940a04aa1..80e6861f6 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -209,12 +209,18 @@ int led_init() /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */ (void)ioctl(leds, LED_ON, LED_BLUE); + /* switch blue off */ + led_off(LED_BLUE); + /* we consider the amber led mandatory */ if (ioctl(leds, LED_ON, LED_AMBER)) { warnx("Amber LED: ioctl fail\n"); return ERROR; } + /* switch amber off */ + led_off(LED_AMBER); + /* then try RGB LEDs, this can fail on FMUv1*/ rgbleds = open(RGBLED_DEVICE_PATH, 0); -- cgit v1.2.3 From 211c721b48c5856ace7f09e88706d799299ca6bb Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 24 Jun 2014 17:42:25 -0400 Subject: Shorten the reset param name to INAV_W_XY_RES_V --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 ++-- .../position_estimator_inav/position_estimator_inav_params.c | 6 +++--- .../position_estimator_inav/position_estimator_inav_params.h | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 5f132453b..eca406acf 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -918,8 +918,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } else { /* gradually reset xy velocity estimates */ - inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_reset_v); - inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_reset_v); + inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); + inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } /* detect land */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index b79e00274..47c1a4c6c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -46,7 +46,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_RESET_V, 0.2f); +PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.2f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); @@ -66,7 +66,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); - h->w_xy_reset_v = param_find("INAV_W_XY_RESET_V"); + h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); @@ -89,7 +89,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); param_get(h->w_xy_flow, &(p->w_xy_flow)); - param_get(h->w_xy_reset_v, &(p->w_xy_reset_v)); + param_get(h->w_xy_res_v, &(p->w_xy_res_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 9be13637d..423f0d879 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -47,7 +47,7 @@ struct position_estimator_inav_params { float w_xy_gps_p; float w_xy_gps_v; float w_xy_flow; - float w_xy_reset_v; + float w_xy_res_v; float w_gps_flow; float w_acc_bias; float flow_k; @@ -67,7 +67,7 @@ struct position_estimator_inav_param_handles { param_t w_xy_gps_p; param_t w_xy_gps_v; param_t w_xy_flow; - param_t w_xy_reset_v; + param_t w_xy_res_v; param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; -- cgit v1.2.3 From 7a45888e78b4af0c04014c20e579797b2040edf8 Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 24 Jun 2014 17:56:36 -0400 Subject: Adjusted default reset param value to reset quicker --- src/modules/position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 47c1a4c6c..0581f8236 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -46,7 +46,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.2f); +PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); -- cgit v1.2.3 From f9d5cf332c9e60661c2e1c0827c84480e49d4fe8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 25 Jun 2014 17:05:20 +0200 Subject: Revert "Hotfix: Only orb_copy items in mavlink app if the timestamp changed" This reverts commit a9653fa10db3884d3d17ee33f80f23aa2e3ef842. --- src/modules/mavlink/mavlink_orb_subscription.cpp | 26 ++++++++++-------------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 3807323c2..901fa8f05 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -44,8 +44,6 @@ #include #include -#include - #include "mavlink_orb_subscription.h" MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : @@ -79,23 +77,21 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data) time_topic = 0; } - if (time_topic != *time) { - - if (orb_copy(_topic, _fd, data)) { - /* error copying topic data */ - memset(data, 0, _topic->o_size); - //warnx("err copy, fd: %d, obj: %s, size: %d", _fd, _topic->o_name, _topic->o_size); - return false; + if (orb_copy(_topic, _fd, data)) { + /* error copying topic data */ + memset(data, 0, _topic->o_size); + return false; - } else { - /* data copied successfully */ - _published = true; + } else { + /* data copied successfully */ + _published = true; + if (time_topic != *time) { *time = time_topic; return true; - } - } else { - return false; + } else { + return false; + } } } -- cgit v1.2.3 From 9d3d5a30af0781b975d17c5d73796f593c78b6e4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 25 Jun 2014 17:13:47 +0200 Subject: navigator: move set_previous_pos_sp to MissionBlock class --- src/modules/navigator/mission.cpp | 12 +----------- src/modules/navigator/mission.h | 6 ------ src/modules/navigator/mission_block.cpp | 9 +++++++++ src/modules/navigator/mission_block.h | 5 +++++ 4 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9244063b1..becaa09e8 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -223,7 +223,7 @@ Mission::advance_mission() void Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) { - set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); + set_previous_pos_setpoint(pos_sp_triplet); /* try setting onboard mission item */ if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { @@ -261,16 +261,6 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) } } -void -Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp, - struct position_setpoint_s *previous_pos_sp) -{ - /* reuse current setpoint as previous setpoint */ - if (current_pos_sp->valid) { - memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s)); - } -} - bool Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 322aaf96a..842c8c9ee 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -106,12 +106,6 @@ private: */ void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet); - /** - * Set previous position setpoint - */ - void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp, - struct position_setpoint_s *previous_pos_sp); - /** * Try to set the current position setpoint from an onboard mission item * @return true if mission item successfully set diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 08576750c..a8eec92ca 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -194,6 +194,15 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite } } +void +MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + /* reuse current setpoint as previous setpoint */ + if (pos_sp_triplet->current.valid) { + memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s)); + } +} + bool MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet) { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 47e6fe81f..a740deea4 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -82,6 +82,11 @@ public: */ void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); + /** + * Set previous position setpoint to current setpoint + */ + void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet); + /** * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position * -- cgit v1.2.3 From bdf1b9274c369ddfd52bcb92952f2467abd7c26f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 25 Jun 2014 17:56:11 +0200 Subject: commander: modes fallback and reject messages fixed --- src/modules/commander/commander.cpp | 30 ++++++++++++++++++++++++++---- 1 file changed, 26 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b957ad9b5..ec4273fc3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1613,10 +1613,10 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTCTL print_reject_mode(status, "POSCTL"); } + // fallback to ALTCTL res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -1627,7 +1627,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin print_reject_mode(status, "ALTCTL"); } - // else fallback to MANUAL + // fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -1639,28 +1639,50 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_RTL"); + + // fallback to LOITER if home position not set + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + } else if (sp_man->loiter_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_LOITER"); + } else { res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_MISSION"); } - // else fallback to ALTCTL (POSCTL likely will not work too) + // fallback to POSCTL + res = main_state_transition(status, MAIN_STATE_POSCTL); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + + // fallback to ALTCTL res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to MANUAL + // fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; -- cgit v1.2.3 From 39454ca99d984698983404b2c82bbd61f93be3fc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 25 Jun 2014 17:56:30 +0200 Subject: navigator: RTL return altitude fixed --- src/modules/navigator/rtl.cpp | 63 ++++++++++++++++++++++--------------------- 1 file changed, 33 insertions(+), 30 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index c1b1d3f09..46194ed52 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -44,6 +44,7 @@ #include #include +#include #include #include @@ -112,21 +113,29 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) if (_navigator->get_vstatus()->condition_landed) { _rtl_state = RTL_STATE_FINISHED; mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + /* if lower than return altitude, climb up first */ } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) { _rtl_state = RTL_STATE_CLIMB; + /* otherwise go straight to return */ } else { + /* set altitude setpoint to current altitude */ _rtl_state = RTL_STATE_RETURN; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; } } - /* if switching directly to return state, set altitude setpoint to current altitude */ - if (_rtl_state == RTL_STATE_RETURN) { - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_global_position()->alt; - } + if (_rtl_state == RTL_STATE_FINISHED) { + /* RTL finished, nothing to do */ + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + return; + } + + set_previous_pos_setpoint(pos_sp_triplet); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -157,17 +166,20 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; - - /* TODO: add this again */ - // don't change altitude - // if (_pos_sp_triplet.previous.valid) { - // /* if previous setpoint is valid then use it to calculate heading to home */ - // _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); - - // } else { - // /* else use current position */ - // _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); - // } + // don't change altitude + + if (pos_sp_triplet->previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + _mission_item.yaw = get_bearing_to_next_waypoint( + pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, + _mission_item.lat, _mission_item.lon); + + } else { + /* else use current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + } _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; @@ -228,24 +240,15 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) break; } - case RTL_STATE_FINISHED: { - /* nothing to do, report fail */ - } - default: break; } - if (_rtl_state == RTL_STATE_FINISHED) { - pos_sp_triplet->current.valid = false; - pos_sp_triplet->next.valid = false; - } else { - /* if not finished, convert mission item to current position setpoint and make it valid */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - reset_mission_item_reached(); - pos_sp_triplet->current.valid = true; - pos_sp_triplet->next.valid = false; - } + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + reset_mission_item_reached(); + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; } void -- cgit v1.2.3 From c5a5604ae975294347f1571258a633c11ef61261 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Jun 2014 00:17:25 +0200 Subject: navigator: loiter fixes --- src/modules/navigator/loiter.cpp | 3 +-- src/modules/navigator/mission.cpp | 8 +++---- src/modules/navigator/mission_block.cpp | 40 ++++++++++++++++++-------------- src/modules/navigator/mission_block.h | 3 +-- src/modules/navigator/navigator.h | 6 ++--- src/modules/navigator/navigator_main.cpp | 4 ++-- src/modules/navigator/rtl.cpp | 9 +------ 7 files changed, 34 insertions(+), 39 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 8bbb79d5e..f5eb1e7a5 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -70,11 +70,10 @@ bool Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { /* set loiter item, don't reuse an existing position setpoint */ - return set_loiter_item(false, pos_sp_triplet);; + return set_loiter_item(pos_sp_triplet); } void Loiter::on_inactive() { } - diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index becaa09e8..c09f61744 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -233,7 +233,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: onboard mission running"); } _mission_type = MISSION_TYPE_ONBOARD; - _navigator->set_is_in_loiter(false); + _navigator->set_can_loiter_at_sp(false); /* try setting offboard mission item */ } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { @@ -243,7 +243,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: offboard mission running"); } _mission_type = MISSION_TYPE_OFFBOARD; - _navigator->set_is_in_loiter(false); + _navigator->set_can_loiter_at_sp(false); } else { if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_info(_navigator->get_mavlink_fd(), @@ -253,9 +253,9 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: no mission available"); } _mission_type = MISSION_TYPE_NONE; + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); - bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached; - set_loiter_item(use_current_pos_sp, pos_sp_triplet); + set_loiter_item(pos_sp_triplet); reset_mission_item_reached(); report_mission_finished(); } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index a8eec92ca..41d81ad9b 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -204,31 +204,35 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_ } bool -MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet) +MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) { - if (_navigator_priv->get_is_in_loiter()) { - /* already loitering, bail out */ - return false; - } - - if (reuse_current_pos_sp && pos_sp_triplet->current.valid) { - /* leave position setpoint as is */ - } else { + /* don't change setpoint if 'can_loiter_at_sp' flag set */ + if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { /* use current position */ pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat; pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon; pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt; pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */ + + _navigator_priv->set_can_loiter_at_sp(true); } - pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; - pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius(); - pos_sp_triplet->current.loiter_direction = 1; - pos_sp_triplet->previous.valid = false; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->next.valid = false; + if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER + || pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius() + || pos_sp_triplet->current.loiter_direction != 1 + || pos_sp_triplet->previous.valid + || !pos_sp_triplet->current.valid + || pos_sp_triplet->next.valid) { + /* position setpoint triplet should be updated */ + pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; + pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius(); + pos_sp_triplet->current.loiter_direction = 1; + + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; + return true; + } - _navigator_priv->set_is_in_loiter(true); - return true; + return false; } - diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index a740deea4..8726964fa 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -90,11 +90,10 @@ public: /** * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position * - * @param true if the current position setpoint should be re-used * @param the position setpoint triplet to set * @return true if setpoint has changed */ - bool set_loiter_item(const bool reuse_current_pos_sp, position_setpoint_triplet_s *pos_sp_triplet); + bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet); bool _waypoint_position_reached; bool _waypoint_yaw_reached; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index dadd15527..4709f7196 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -102,7 +102,7 @@ public: /** * Setters */ - void set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; } + void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } /** * Getters @@ -113,7 +113,7 @@ public: int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } Geofence& get_geofence() { return _geofence; } - bool get_is_in_loiter() { return _is_in_loiter; } + bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); } int get_mavlink_fd() { return _mavlink_fd; } @@ -161,7 +161,7 @@ private: NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ - bool _is_in_loiter; /**< flags if current position SP can be used to loiter */ + bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ bool _update_triplet; /**< flags if position SP triplet needs to be published */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d762b8a9d..f39300590 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -347,7 +347,7 @@ Navigator::task_main() case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: _navigation_mode = nullptr; - _is_in_loiter = false; + _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: _navigation_mode = &_mission; @@ -365,7 +365,7 @@ Navigator::task_main() case NAVIGATION_STATE_TERMINATION: default: _navigation_mode = nullptr; - _is_in_loiter = false; + _can_loiter_at_sp = false; break; } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 46194ed52..411f8c527 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -136,6 +136,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) } set_previous_pos_setpoint(pos_sp_triplet); + _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -156,8 +157,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - _navigator->set_is_in_loiter(false); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", (int)(climb_alt - _navigator->get_home_position()->alt)); break; @@ -189,8 +188,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - _navigator->set_is_in_loiter(false); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; @@ -211,8 +208,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.autocontinue = _param_land_delay.get() > -0.001f; _mission_item.origin = ORIGIN_ONBOARD; - _navigator->set_is_in_loiter(true); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; @@ -234,8 +229,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - _navigator->set_is_in_loiter(false); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); break; } -- cgit v1.2.3 From 831a3d4ed14bc068ee086c5190af96ad809f67d7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 26 Jun 2014 07:47:46 +0200 Subject: mtecs: improve logic readability --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 03353cbc1..48c9079a4 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -199,8 +199,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight } /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ - BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits - BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits + BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); + BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); if (mode == TECS_MODE_TAKEOFF) { outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; @@ -218,12 +218,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector * is running) */ - bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ? - _controlTotalEnergy.getOutputLimiter() : - *outputLimiterThrottle, - outputLimiterPitch == NULL ? - _controlEnergyDistribution.getOutputLimiter() : - *outputLimiterPitch); + bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); /* Write part of the status message */ _status.airspeedDerivativeSp = airspeedDerivativeSp; -- cgit v1.2.3 From 1cd82fc89c70060947683851af28556bccd675a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:07:28 +0200 Subject: Add mavlink_fd to all commander arm transitions to provide user feedback why the arming command failed --- src/modules/commander/commander.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 65922b2a5..588f48225 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -412,7 +412,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* if HIL got enabled, reset battery status state */ if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd); if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); @@ -945,8 +945,8 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch"); } } } @@ -1149,10 +1149,10 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); } status_changed = true; @@ -1163,7 +1163,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1217,7 +1217,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); stick_off_counter = 0; } else { @@ -1239,7 +1239,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); } stick_on_counter = 0; @@ -1941,7 +1941,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2004,7 +2004,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); break; } -- cgit v1.2.3 From 435cacb0a0c42b9995edc57cc33e68ff324e6106 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:08:25 +0200 Subject: commander: Make mavlink_fd in arming command non-optional --- src/modules/commander/state_machine_helper.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0ddd4f05a..abb917873 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); -- cgit v1.2.3 From 43c3559763841abacf5e74e15a6172026071088b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:08:25 +0200 Subject: commander: Make mavlink_fd in arming command non-optional --- src/modules/commander/state_machine_helper.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0ddd4f05a..abb917873 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); -- cgit v1.2.3 From 547f71d791d0a04001580e8371bdf59b808a3d29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:07:28 +0200 Subject: Add mavlink_fd to all commander arm transitions to provide user feedback why the arming command failed --- src/modules/commander/commander.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 65922b2a5..588f48225 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -412,7 +412,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* if HIL got enabled, reset battery status state */ if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd); if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); @@ -945,8 +945,8 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch"); } } } @@ -1149,10 +1149,10 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); } status_changed = true; @@ -1163,7 +1163,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1217,7 +1217,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); stick_off_counter = 0; } else { @@ -1239,7 +1239,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); } stick_on_counter = 0; @@ -1941,7 +1941,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2004,7 +2004,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); break; } -- cgit v1.2.3 From a1132580c55512a56ec7646f757099409ebad781 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:11:39 +0200 Subject: commander: Add pre-arm check for main sensors. --- src/modules/commander/state_machine_helper.cpp | 135 +++++++++++++++++++++---- 1 file changed, 118 insertions(+), 17 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1a5a52a02..6816d46ab 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -46,14 +46,18 @@ #include #include #include +#include #include #include #include +#include #include #include #include #include +#include +#include #include #include @@ -66,6 +70,8 @@ #endif static const int ERROR = -1; +static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); + static bool arming_state_changed = true; static bool main_state_changed = true; static bool failsafe_state_changed = true; @@ -108,18 +114,31 @@ arming_state_transition(struct vehicle_status_s *status, /// current ASSERT(ARMING_STATE_INIT == 0); ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); - /* - * Perform an atomic state update - */ - irqstate_t flags = irqsave(); - transition_result_t ret = TRANSITION_DENIED; + arming_state_t current_arming_state = status->arming_state; + /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == status->arming_state) { + if (new_arming_state == current_arming_state) { ret = TRANSITION_NOT_CHANGED; } else { + + /* + * Get sensing state if necessary + */ + int prearm_ret = OK; + + /* only perform the check if we have to */ + if (new_arming_state == ARMING_STATE_ARMED) { + prearm_ret = prearm_check(status, mavlink_fd); + } + + /* + * Perform an atomic state update + */ + irqstate_t flags = irqsave(); + /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; @@ -139,12 +158,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF) { + // Fail transition if pre-arm check fails + if (prearm_ret) { + valid_transition = false; + // Fail transition if we need safety switch press - if (safety->safety_switch_available && !safety->safety_off) { + } else if (safety->safety_switch_available && !safety->safety_off) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); - } + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); valid_transition = false; } @@ -173,17 +194,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current status->arming_state = new_arming_state; arming_state_changed = true; } - } - /* end of atomic state update */ - irqrestore(flags); + /* end of atomic state update */ + irqrestore(flags); + } if (ret == TRANSITION_DENIED) { - static const char *errMsg = "Invalid arming transition from %s to %s"; + static const char *errMsg = "INVAL: %s - %s"; - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); - } + mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); } @@ -496,6 +515,88 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f return ret; } +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) +{ + warnx("PREARM"); + + int ret; + + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); + ret = fd; + goto system_eval; + } + + ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + goto system_eval; + } + + /* check measurement result range */ + struct accel_report acc; + ret = read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + + if (accel_scale < 9.78f || accel_scale > 9.83f) { + mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); + } + + if (accel_scale > 30.0f /* m/s^2 */) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); + /* this is frickin' fatal */ + ret = ERROR; + goto system_eval; + } else { + ret = OK; + } + } else { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); + /* this is frickin' fatal */ + ret = ERROR; + goto system_eval; + } + + if (!status->is_rotary_wing) { + int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); + ret = fd; + goto system_eval; + } + + struct differential_pressure_s diff_pres; + + ret = read(fd, &diff_pres, sizeof(diff_pres)); + + if (ret == sizeof(diff_pres)) { + if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { + mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + // XXX do not make this fatal yet + ret = OK; + } + } else { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); + /* this is frickin' fatal */ + ret = ERROR; + goto system_eval; + } + } + + warnx("PRE RES: %d", ret); + +system_eval: + close(fd); + return ret; +} + // /* -- cgit v1.2.3 From 690edbd6723746699067f69fd3c76d4d3806d9c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:14:49 +0200 Subject: Remove duplicate code in the arming check --- src/modules/commander/commander.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 588f48225..8e9352394 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1232,12 +1232,13 @@ int commander_thread_main(int argc, char *argv[]) if (status.arming_state == ARMING_STATE_STANDBY && sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { - print_reject_arm("#audio: NOT ARMING: Press safety switch first."); - } else if (status.main_state != MAIN_STATE_MANUAL) { + /* we check outside of the transition function here because the requirement + * for being in manual mode only applies to manual arming actions. + * the system can be armed in auto if armed via the GCS. + */ + if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); - } else { arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); } -- cgit v1.2.3 From 63e14c73bab8f5b14b5259ba249f84a8edb0ee05 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Jun 2014 12:18:19 +0200 Subject: navigator: don't reset RTL state on loiter --- src/modules/navigator/rtl.cpp | 58 +++++++++++++++++++++++-------------------- 1 file changed, 31 insertions(+), 27 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 411f8c527..a2eec8589 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -76,7 +76,11 @@ void RTL::on_inactive() { _first_run = true; - _rtl_state = RTL_STATE_NONE; + + /* reset RTL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _rtl_state = RTL_STATE_NONE; + } } bool @@ -85,14 +89,35 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) bool updated = false; if (_first_run) { + _first_run = false; + + /* decide where to enter the RTL procedure when we switch into it */ + if (_rtl_state == RTL_STATE_NONE) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_FINISHED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + /* if lower than return altitude, climb up first */ + } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + /* otherwise go straight to return */ + } else { + /* set altitude setpoint to current altitude */ + _rtl_state = RTL_STATE_RETURN; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; + } + } + set_rtl_item(pos_sp_triplet); + updated = true; - _first_run = false; } - if ((_rtl_state == RTL_STATE_CLIMB - || _rtl_state == RTL_STATE_RETURN) - && is_mission_item_reached()) { + if ((_rtl_state != RTL_STATE_FINISHED) && is_mission_item_reached()) { advance_rtl(); set_rtl_item(pos_sp_triplet); updated = true; @@ -107,31 +132,10 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) /* make sure we have the latest params */ updateParams(); - /* decide where to enter the RTL procedure when we switch into it */ - if (_rtl_state == RTL_STATE_NONE) { - /* for safety reasons don't go into RTL if landed */ - if (_navigator->get_vstatus()->condition_landed) { - _rtl_state = RTL_STATE_FINISHED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - - /* if lower than return altitude, climb up first */ - } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - + _param_return_alt.get()) { - _rtl_state = RTL_STATE_CLIMB; - - /* otherwise go straight to return */ - } else { - /* set altitude setpoint to current altitude */ - _rtl_state = RTL_STATE_RETURN; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_global_position()->alt; - } - } - if (_rtl_state == RTL_STATE_FINISHED) { /* RTL finished, nothing to do */ - pos_sp_triplet->current.valid = false; pos_sp_triplet->next.valid = false; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed"); return; } -- cgit v1.2.3 From b20d0c0f015fb62955f7a399b50626a0d57c5f64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:36:43 +0200 Subject: commander: Remove prearm test print statements --- src/modules/commander/state_machine_helper.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6816d46ab..8ce724b4c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -517,8 +517,6 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { - warnx("PREARM"); - int ret; int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -590,8 +588,6 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } } - warnx("PRE RES: %d", ret); - system_eval: close(fd); return ret; -- cgit v1.2.3 From 6e5aafb3a753f83b1db936e94de09144ee115c0d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Jun 2014 13:44:41 +0200 Subject: navigator: minor formatting fix --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f39300590..493e86d4a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -386,7 +386,7 @@ Navigator::task_main() _update_triplet = true; } - if (_update_triplet ) { + if (_update_triplet) { publish_position_setpoint_triplet(); _update_triplet = false; } -- cgit v1.2.3 From 1a0f53ec3a95c35106f63d58b0ca0b5e7b816d89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 13:50:46 +0200 Subject: mavlink-ftp: Remove two extra bytes we don not need --- src/modules/mavlink/mavlink_ftp.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 0869a5fdb..34b67b87f 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -165,7 +165,7 @@ private: // unsigned len = mavlink_msg_system_time_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), // _mavlink->get_channel(), &msg, 255, 255); - if (!_mavlink->message_buffer_write(&msg, len+2)) { + if (!_mavlink->message_buffer_write(&msg, len)) { warnx("FTP TX ERR"); } else { warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", -- cgit v1.2.3 From f9db1723e3899589c10f4f8847c3db1029f999cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 14:20:29 +0200 Subject: mavlink-ftp: Remove commented out code --- src/modules/mavlink/mavlink_ftp.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 34b67b87f..462594301 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -162,8 +162,6 @@ private: msg.checksum = 0; unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), _mavlink->get_channel(), &msg, sequence(), rawData()); - // unsigned len = mavlink_msg_system_time_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), - // _mavlink->get_channel(), &msg, 255, 255); if (!_mavlink->message_buffer_write(&msg, len)) { warnx("FTP TX ERR"); -- cgit v1.2.3 From 7546b99a24a33ff160b29b5be31a9e0d39bbce1a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 14:26:35 +0200 Subject: mavlink-ftp: Add extra padding because the ringbuffer implementation relies on its use --- src/modules/mavlink/mavlink_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 647519ea5..e9cce175d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1903,8 +1903,11 @@ Mavlink::task_main(int argc, char *argv[]) /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ if (_passing_on || _ftp_on) { - /* initialize message buffer if multiplexing is on */ - if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN)) { + /* initialize message buffer if multiplexing is on or its needed for FTP. + * make space for two messages plus off-by-one space as we use the empty element + * marker ring buffer approach. + */ + if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) { errx(1, "can't allocate message buffer, exiting"); } -- cgit v1.2.3 From f76040e55407d3526fb7655ec732304c9ba1dc8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:54:33 +0200 Subject: mavlink: Always send heartbeat and do not require both topics to update --- src/modules/mavlink/mavlink_messages.cpp | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4433377c6..b295bf35f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -226,19 +226,21 @@ protected: struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; - if (status_sub->update(&status) && pos_sp_triplet_sub->update(&pos_sp_triplet)) { - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - mavlink_msg_heartbeat_send(_channel, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); - } + /* always send the heartbeat, independent of the update status of the topics */ + (void)status_sub->update(&status); + (void)pos_sp_triplet_sub->update(&pos_sp_triplet); + + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + mavlink_msg_heartbeat_send(_channel, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); } }; -- cgit v1.2.3 From 62156e78ae45e1c5e2a5ecc305b30cf1b4e0e7f2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Jun 2014 22:37:11 +0200 Subject: mavlink: init structs for HEARTBEAT if uORB topic copy failed --- src/modules/mavlink/mavlink_messages.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b295bf35f..97520fce9 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -227,8 +227,15 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; /* always send the heartbeat, independent of the update status of the topics */ - (void)status_sub->update(&status); - (void)pos_sp_triplet_sub->update(&pos_sp_triplet); + if (!status_sub->update(&status)) { + /* if topic update failed fill it with defaults */ + memset(&status, 0, sizeof(status)); + } + + if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) { + /* if topic update failed fill it with defaults */ + memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + } uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; -- cgit v1.2.3 From 9e5c94b41e6e4d0b5869c80ee4f6248bedffe7b4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Jun 2014 22:41:35 +0200 Subject: mavlink: spaces replaced with tabs --- src/modules/mavlink/mavlink_messages.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 97520fce9..ef9039a6a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -228,13 +228,13 @@ protected: /* always send the heartbeat, independent of the update status of the topics */ if (!status_sub->update(&status)) { - /* if topic update failed fill it with defaults */ - memset(&status, 0, sizeof(status)); + /* if topic update failed fill it with defaults */ + memset(&status, 0, sizeof(status)); } if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) { - /* if topic update failed fill it with defaults */ - memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + /* if topic update failed fill it with defaults */ + memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); } uint8_t mavlink_state = 0; -- cgit v1.2.3 From 8e8798a5225d333596c9018ec703da0c6787493d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Jun 2014 23:39:17 +0200 Subject: navigator: spaces/tabs fixed, old commented code removed --- src/modules/navigator/navigator_main.cpp | 322 ------------------------------- src/modules/navigator/rtl.cpp | 72 +++---- 2 files changed, 36 insertions(+), 358 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 493e86d4a..e22c4d72d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -446,329 +446,7 @@ Navigator::status() warnx("Geofence not set"); } } -#if 0 -bool -Navigator::start_none_on_ground() -{ - reset_reached(); - - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.next.valid = false; - - _update_triplet = true; - return true; -} - -bool -Navigator::start_none_in_air() -{ - reset_reached(); - - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.next.valid = false; - - _update_triplet = true; - return true; -} - -bool -Navigator::start_auto_on_ground() -{ - reset_reached(); - - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.next.valid = false; - - _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; - - _update_triplet = true; - return true; -} - -bool -Navigator::start_loiter() -{ - /* if no existing item available, use current position */ - if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) { - - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - _pos_sp_triplet.current.alt = _global_pos.alt; - } - _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; - _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; - _pos_sp_triplet.current.loiter_direction = 1; - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); - - _update_triplet = true; - return true; -} - -bool -Navigator::start_mission() -{ - /* start fresh */ - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.next.valid = false; - - return set_mission_items(); -} - -bool -Navigator::advance_mission() -{ - /* tell mission to move by one */ - _mission.move_to_next(); - - /* now try to set the new mission items, if it fails, it will dispatch loiter */ - return set_mission_items(); -} - -bool -Navigator::set_mission_items() -{ - if (_pos_sp_triplet.current.valid) { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _pos_sp_triplet.previous.valid = true; - } - - bool onboard; - int index; - - /* if we fail to set the current mission, continue to loiter */ - if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) { - - return false; - } - - /* if we got an RTL mission item, switch to RTL mode and give up */ - if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - return false; - } - - _mission_item_valid = true; - - /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); - _pos_sp_triplet.current.valid = true; - - - mission_item_s next_mission_item; - - bool last_wp = false; - /* now try to set the next mission item as well, if there is no more next - * this means we're heading to the last waypoint */ - if (_mission.get_next_mission_item(&next_mission_item)) { - /* convert the next mission item and set it valid */ - mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next); - _pos_sp_triplet.next.valid = true; - } else { - last_wp = true; - } - - /* notify user about what happened */ - mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d", - (last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index); - - _update_triplet = true; - - reset_reached(); - - return true; -} - -bool -Navigator::start_rtl() -{ - if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - - _mission_item_valid = true; - - mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); - _pos_sp_triplet.current.valid = true; - - reset_reached(); - - _update_triplet = true; - return true; - } - - /* if RTL doesn't work, fallback to loiter */ - return false; -} - -bool -Navigator::advance_rtl() -{ - /* tell mission to move by one */ - _rtl.move_to_next(); - /* now try to set the new mission items, if it fails, it will dispatch loiter */ - if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - - _mission_item_valid = true; - - mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); - _pos_sp_triplet.current.valid = true; - - reset_reached(); - - _update_triplet = true; - return true; - } - - return false; -} - -bool -Navigator::start_land() -{ - /* TODO: verify/test */ - - reset_reached(); - - /* this state can be requested by commander even if no global position available, - * in his case controller must perform landing without position control */ - - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - _mission_item_valid = true; - - mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); - - _pos_sp_triplet.next.valid = false; - - _update_triplet = true; - return true; -} -bool -Navigator::check_mission_item_reached() -{ - /* only check if there is actually a mission item to check */ - if (!_mission_item_valid) { - return false; - } - - if (_mission_item.nav_cmd == NAV_CMD_LAND) { - return _vstatus.condition_landed; - } - - /* XXX TODO count turns */ - if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item.loiter_radius > 0.01f) { - - // return false; - // } - - uint64_t now = hrt_absolute_time(); - - if (!_waypoint_position_reached) { - float acceptance_radius; - - if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item.acceptance_radius; - - } else { - acceptance_radius = _parameters.acceptance_radius; - } - - if (_do_takeoff) { - /* require only altitude for takeoff */ - if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) { - _waypoint_position_reached = true; - } - } else { - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; - - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); - - if (dist >= 0.0f && dist <= acceptance_radius) { - _waypoint_position_reached = true; - } - } - } - - if (_waypoint_position_reached && !_waypoint_yaw_reached) { - - /* TODO: removed takeoff, why? */ - if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) { - - /* check yaw if defined only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); - - if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ - _waypoint_yaw_reached = true; - } - - } else { - _waypoint_yaw_reached = true; - } - } - - /* check if the current waypoint was reached */ - if (_waypoint_position_reached && _waypoint_yaw_reached) { - - if (_time_first_inside_orbit == 0) { - _time_first_inside_orbit = now; - - if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", - (double)_mission_item.time_inside); - } - } - - /* check if the MAV was long enough inside the waypoint orbit */ - if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) - || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - return true; - } - } - return false; -} - -void -Navigator::reset_reached() -{ - _time_first_inside_orbit = 0; - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - -} -#endif void Navigator::publish_position_setpoint_triplet() { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index a2eec8589..c5bb06c3b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -79,7 +79,7 @@ RTL::on_inactive() /* reset RTL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { - _rtl_state = RTL_STATE_NONE; + _rtl_state = RTL_STATE_NONE; } } @@ -89,28 +89,28 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) bool updated = false; if (_first_run) { - _first_run = false; - - /* decide where to enter the RTL procedure when we switch into it */ - if (_rtl_state == RTL_STATE_NONE) { - /* for safety reasons don't go into RTL if landed */ - if (_navigator->get_vstatus()->condition_landed) { - _rtl_state = RTL_STATE_FINISHED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - - /* if lower than return altitude, climb up first */ - } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - + _param_return_alt.get()) { - _rtl_state = RTL_STATE_CLIMB; - - /* otherwise go straight to return */ - } else { - /* set altitude setpoint to current altitude */ - _rtl_state = RTL_STATE_RETURN; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_global_position()->alt; - } - } + _first_run = false; + + /* decide where to enter the RTL procedure when we switch into it */ + if (_rtl_state == RTL_STATE_NONE) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_FINISHED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + /* if lower than return altitude, climb up first */ + } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + /* otherwise go straight to return */ + } else { + /* set altitude setpoint to current altitude */ + _rtl_state = RTL_STATE_RETURN; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; + } + } set_rtl_item(pos_sp_triplet); @@ -132,15 +132,15 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) /* make sure we have the latest params */ updateParams(); - if (_rtl_state == RTL_STATE_FINISHED) { - /* RTL finished, nothing to do */ - pos_sp_triplet->next.valid = false; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed"); - return; - } + if (_rtl_state == RTL_STATE_FINISHED) { + /* RTL finished, nothing to do */ + pos_sp_triplet->next.valid = false; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed"); + return; + } - set_previous_pos_setpoint(pos_sp_triplet); - _navigator->set_can_loiter_at_sp(false); + set_previous_pos_setpoint(pos_sp_triplet); + _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -241,11 +241,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) break; } - /* convert mission item to current position setpoint and make it valid */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - reset_mission_item_reached(); - pos_sp_triplet->current.valid = true; - pos_sp_triplet->next.valid = false; + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + reset_mission_item_reached(); + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; } void -- cgit v1.2.3 From 9ae44291b169a90945f090487e3757db9fc8c075 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Jun 2014 00:27:08 +0200 Subject: navigator: added NAV_CMD_IDLE, added RTL_STATE_LOITER and RTL_STATE_LANDED instead of FINISHED --- src/modules/navigator/mission_block.cpp | 25 ++++++---- src/modules/navigator/rtl.cpp | 84 ++++++++++++++++++++++++--------- src/modules/navigator/rtl.h | 3 +- src/modules/uORB/topics/mission.h | 1 + 4 files changed, 82 insertions(+), 31 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 41d81ad9b..5e545706d 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -69,10 +69,10 @@ MissionBlock::~MissionBlock() bool MissionBlock::is_mission_item_reached() { - /* don't check landed WPs */ if (_mission_item.nav_cmd == NAV_CMD_LAND) { - return false; + return _navigator_priv->get_vstatus()->condition_landed; } + /* TODO: count turns */ #if 0 if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || @@ -178,19 +178,28 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->loiter_direction = item->loiter_direction; sp->pitch_min = item->pitch_min; - if (item->nav_cmd == NAV_CMD_TAKEOFF) { + switch (item->nav_cmd) { + case NAV_CMD_IDLE: + sp->type = SETPOINT_TYPE_IDLE; + break; + + case NAV_CMD_TAKEOFF: sp->type = SETPOINT_TYPE_TAKEOFF; + break; - } else if (item->nav_cmd == NAV_CMD_LAND) { + case NAV_CMD_LAND: sp->type = SETPOINT_TYPE_LAND; + break; - } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + case NAV_CMD_LOITER_TIME_LIMIT: + case NAV_CMD_LOITER_TURN_COUNT: + case NAV_CMD_LOITER_UNLIMITED: sp->type = SETPOINT_TYPE_LOITER; + break; - } else { + default: sp->type = SETPOINT_TYPE_POSITION; + break; } } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index c5bb06c3b..b7961a260 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -53,6 +53,8 @@ #include "navigator.h" #include "rtl.h" +#define DELAY_SIGMA 0.01f + RTL::RTL(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), MissionBlock(navigator), @@ -95,7 +97,7 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) if (_rtl_state == RTL_STATE_NONE) { /* for safety reasons don't go into RTL if landed */ if (_navigator->get_vstatus()->condition_landed) { - _rtl_state = RTL_STATE_FINISHED; + _rtl_state = RTL_STATE_LANDED; mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); /* if lower than return altitude, climb up first */ @@ -113,11 +115,9 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) } set_rtl_item(pos_sp_triplet); - updated = true; - } - if ((_rtl_state != RTL_STATE_FINISHED) && is_mission_item_reached()) { + } else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { advance_rtl(); set_rtl_item(pos_sp_triplet); updated = true; @@ -132,19 +132,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) /* make sure we have the latest params */ updateParams(); - if (_rtl_state == RTL_STATE_FINISHED) { - /* RTL finished, nothing to do */ - pos_sp_triplet->next.valid = false; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed"); - return; - } - set_previous_pos_setpoint(pos_sp_triplet); _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { case RTL_STATE_CLIMB: { - float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); _mission_item.lat = _navigator->get_global_position()->lat; @@ -165,8 +157,8 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) (int)(climb_alt - _navigator->get_home_position()->alt)); break; } - case RTL_STATE_RETURN: { + case RTL_STATE_RETURN: { _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; // don't change altitude @@ -196,8 +188,8 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } - case RTL_STATE_DESCEND: { + case RTL_STATE_DESCEND: { _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; @@ -207,9 +199,9 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _param_acceptance_radius.get(); - _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = _param_land_delay.get() > -0.001f; + _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", @@ -217,8 +209,35 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) break; } - case RTL_STATE_LAND: { + case RTL_STATE_LOITER: { + bool autoland = _param_land_delay.get() > -DELAY_SIGMA; + + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = autoland; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + if (autoland) { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside); + + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); + } + break; + } + + case RTL_STATE_LAND: { _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; @@ -237,6 +256,25 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) break; } + case RTL_STATE_LANDED: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_IDLE; + _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed"); + break; + } + default: break; } @@ -262,18 +300,20 @@ RTL::advance_rtl() case RTL_STATE_DESCEND: /* only go to land if autoland is enabled */ - if (_param_land_delay.get() < 0) { - _rtl_state = RTL_STATE_FINISHED; + if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) { + _rtl_state = RTL_STATE_LOITER; + } else { _rtl_state = RTL_STATE_LAND; } break; - case RTL_STATE_LAND: - _rtl_state = RTL_STATE_FINISHED; + case RTL_STATE_LOITER: + _rtl_state = RTL_STATE_LAND; break; - case RTL_STATE_FINISHED: + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_LANDED; break; default: diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index bcccf7454..c4286b391 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -97,8 +97,9 @@ private: RTL_STATE_CLIMB, RTL_STATE_RETURN, RTL_STATE_DESCEND, + RTL_STATE_LOITER, RTL_STATE_LAND, - RTL_STATE_FINISHED, + RTL_STATE_LANDED, } _rtl_state; control::BlockParamFloat _param_return_alt; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index d25db3a77..d9dd61df1 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -50,6 +50,7 @@ /* compatible to mavlink MAV_CMD */ enum NAV_CMD { + NAV_CMD_IDLE=0, NAV_CMD_WAYPOINT=16, NAV_CMD_LOITER_UNLIMITED=17, NAV_CMD_LOITER_TURN_COUNT=18, -- cgit v1.2.3 From 1bae18377a444279f91b75281ffde5da70cc6086 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Jun 2014 00:29:00 +0200 Subject: navigator: is_mission_item_reached() for LOITER items fixed --- src/modules/navigator/mission_block.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 5e545706d..8ef7be449 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -74,15 +74,10 @@ MissionBlock::is_mission_item_reached() } /* TODO: count turns */ -#if 0 - if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item.loiter_radius > 0.01f) { - + if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { return false; } -#endif hrt_abstime now = hrt_absolute_time(); -- cgit v1.2.3 From 52eb49ba0bd1ea5a05845350f1b3c46f0b059a39 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Jun 2014 11:09:49 +0200 Subject: navigator: use common "acceptance radius" parameter for all modes --- src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/navigator/navigator.h | 4 ++-- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/navigator/navigator_params.c | 6 +++--- src/modules/navigator/rtl.cpp | 15 +++++++-------- src/modules/navigator/rtl.h | 1 - src/modules/navigator/rtl_params.c | 12 ------------ 7 files changed, 15 insertions(+), 29 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 8ef7be449..4f177d76e 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -100,12 +100,12 @@ MissionBlock::is_mission_item_reached() if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) { /* require only altitude for takeoff for multicopter */ if (_navigator_priv->get_global_position()->alt > - altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) { + altitude_amsl - _navigator_priv->get_acceptance_radius()) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */ - if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) { + if (dist >= 0.0f && dist <= _navigator_priv->get_acceptance_radius()) { _waypoint_position_reached = true; } } else { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 4709f7196..184ecd365 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -115,7 +115,7 @@ public: Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } - float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); } + float get_acceptance_radius() { return _param_acceptance_radius.get(); } int get_mavlink_fd() { return _mavlink_fd; } private: @@ -165,7 +165,7 @@ private: bool _update_triplet; /**< flags if position SP triplet needs to be published */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ - control::BlockParamFloat _param_takeoff_acceptance_radius; /**< acceptance for takeoff */ + control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e22c4d72d..546602741 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -123,7 +123,7 @@ Navigator::Navigator() : _rtl(this, "RTL"), _update_triplet(false), _param_loiter_radius(this, "LOITER_RAD"), - _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") + _param_acceptance_radius(this, "ACC_RAD") { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index ca31adecc..084afe340 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -55,12 +55,12 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); /** - * Takeoff Acceptance Radius (FW only) + * Acceptance Radius * - * Acceptance radius for fixedwing. + * Default acceptance radius, overridden by acceptance radius of waypoint if set. * * @unit meters * @min 1.0 * @group Mission */ -PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f); +PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b7961a260..af2ff308c 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -61,8 +61,7 @@ RTL::RTL(Navigator *navigator, const char *name) : _rtl_state(RTL_STATE_NONE), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY"), - _param_acceptance_radius(this, "ACCEPT_RAD") + _param_land_delay(this, "LAND_DELAY") { /* load initial params */ updateParams(); @@ -147,7 +146,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; @@ -178,7 +177,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; @@ -198,7 +197,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = false; @@ -220,7 +219,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = autoland; @@ -246,7 +245,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; @@ -265,7 +264,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_IDLE; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index c4286b391..90299c3fa 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -105,7 +105,6 @@ private: control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; - control::BlockParamFloat _param_acceptance_radius; }; #endif diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 63724f461..bfe6ce7e1 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -96,15 +96,3 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); * @group RTL */ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); - -/** - * RTL acceptance radius - * - * Acceptance radius for waypoints set for RTL - * - * @unit meters - * @min 1 - * @max - * @group RTL - */ -PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f); -- cgit v1.2.3 From affc312411b7634fa13bab6da8889de90f964ce8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Jun 2014 11:34:19 +0200 Subject: navigator: make MissionBlock subclass of NavigatorMode --- src/modules/navigator/loiter.cpp | 3 +-- src/modules/navigator/loiter.h | 2 +- src/modules/navigator/mission.cpp | 3 +-- src/modules/navigator/mission.h | 2 +- src/modules/navigator/mission_block.cpp | 44 ++++++++++++++++----------------- src/modules/navigator/mission_block.h | 11 +++------ src/modules/navigator/rtl.cpp | 3 +-- src/modules/navigator/rtl.h | 2 +- 8 files changed, 32 insertions(+), 38 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index f5eb1e7a5..542483fb1 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -53,8 +53,7 @@ #include "loiter.h" Loiter::Loiter(Navigator *navigator, const char *name) : - NavigatorMode(navigator, name), - MissionBlock(navigator) + MissionBlock(navigator, name) { /* load initial params */ updateParams(); diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 685ae6141..65ff5c31e 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -47,7 +47,7 @@ #include "navigator_mode.h" #include "mission_block.h" -class Loiter : public NavigatorMode, MissionBlock +class Loiter : public MissionBlock { public: /** diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c09f61744..72255103b 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -58,8 +58,7 @@ #include "mission.h" Mission::Mission(Navigator *navigator, const char *name) : - NavigatorMode(navigator, name), - MissionBlock(navigator), + MissionBlock(navigator, name), _param_onboard_enabled(this, "ONBOARD_EN"), _onboard_mission({0}), _offboard_mission({0}), diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 842c8c9ee..6e4761946 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -62,7 +62,7 @@ class Navigator; -class Mission : public NavigatorMode, MissionBlock +class Mission : public MissionBlock { public: /** diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4f177d76e..9b8d3d9c7 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -52,13 +52,13 @@ #include "mission_block.h" -MissionBlock::MissionBlock(Navigator *navigator) : +MissionBlock::MissionBlock(Navigator *navigator, const char *name) : + NavigatorMode(navigator, name), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _mission_item({0}), - _mission_item_valid(false), - _navigator_priv(navigator) + _mission_item_valid(false) { } @@ -70,7 +70,7 @@ bool MissionBlock::is_mission_item_reached() { if (_mission_item.nav_cmd == NAV_CMD_LAND) { - return _navigator_priv->get_vstatus()->condition_landed; + return _navigator->get_vstatus()->condition_landed; } /* TODO: count turns */ @@ -88,24 +88,24 @@ MissionBlock::is_mission_item_reached() float dist_z = -1.0f; float altitude_amsl = _mission_item.altitude_is_relative - ? _mission_item.altitude + _navigator_priv->get_home_position()->alt + ? _mission_item.altitude + _navigator->get_home_position()->alt : _mission_item.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, - _navigator_priv->get_global_position()->lat, - _navigator_priv->get_global_position()->lon, - _navigator_priv->get_global_position()->alt, + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, &dist_xy, &dist_z); - if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) { + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) { /* require only altitude for takeoff for multicopter */ - if (_navigator_priv->get_global_position()->alt > - altitude_amsl - _navigator_priv->get_acceptance_radius()) { + if (_navigator->get_global_position()->alt > + altitude_amsl - _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */ - if (dist >= 0.0f && dist <= _navigator_priv->get_acceptance_radius()) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } } else { @@ -119,10 +119,10 @@ MissionBlock::is_mission_item_reached() if (_waypoint_position_reached && !_waypoint_yaw_reached) { /* TODO: removed takeoff, why? */ - if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { + if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw); + float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ _waypoint_yaw_reached = true; @@ -167,7 +167,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->valid = true; sp->lat = item->lat; sp->lon = item->lon; - sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude; + sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; sp->yaw = item->yaw; sp->loiter_radius = item->loiter_radius; sp->loiter_direction = item->loiter_direction; @@ -211,25 +211,25 @@ bool MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) { /* don't change setpoint if 'can_loiter_at_sp' flag set */ - if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { + if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { /* use current position */ - pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat; - pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon; - pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt; + pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */ - _navigator_priv->set_can_loiter_at_sp(true); + _navigator->set_can_loiter_at_sp(true); } if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER - || pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius() + || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius() || pos_sp_triplet->current.loiter_direction != 1 || pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || pos_sp_triplet->next.valid) { /* position setpoint triplet should be updated */ pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; - pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius(); + pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius(); pos_sp_triplet->current.loiter_direction = 1; pos_sp_triplet->previous.valid = false; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 8726964fa..f99002752 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -47,17 +47,17 @@ #include #include +#include "navigator_mode.h" + class Navigator; -class MissionBlock +class MissionBlock : public NavigatorMode { public: /** * Constructor - * - * @param pointer to parent class */ - MissionBlock(Navigator *navigator); + MissionBlock(Navigator *navigator, const char *name); /** * Destructor @@ -101,9 +101,6 @@ public: mission_item_s _mission_item; bool _mission_item_valid; - -private: - Navigator *_navigator_priv; }; #endif diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index af2ff308c..043f773a4 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -56,8 +56,7 @@ #define DELAY_SIGMA 0.01f RTL::RTL(Navigator *navigator, const char *name) : - NavigatorMode(navigator, name), - MissionBlock(navigator), + MissionBlock(navigator, name), _rtl_state(RTL_STATE_NONE), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 90299c3fa..b4b729e89 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -54,7 +54,7 @@ class Navigator; -class RTL : public NavigatorMode, MissionBlock +class RTL : public MissionBlock { public: /** -- cgit v1.2.3 From 5330ffe458bd32bcf0e733c93fa2e659fbaaeeb3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 21 Jun 2014 12:24:37 +0200 Subject: commander: detect if offboard control is lost and missing offboard states --- src/modules/commander/commander.cpp | 14 ++++++++++++++ src/modules/commander/state_machine_helper.cpp | 19 +++++++++++++++++++ 2 files changed, 33 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fab80e325..0c9ce4418 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -661,6 +661,7 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -932,6 +933,19 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } + if (sp_offboard.timestamp != 0 && + sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { + if (status.offboard_control_signal_lost) { + status.offboard_control_signal_lost = false; + status_changed = true; + } + } else { + if (!status.offboard_control_signal_lost) { + status.offboard_control_signal_lost = true; + status_changed = true; + } + } + orb_check(telemetry_sub, &updated); if (updated) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 81b489ce3..4339a7a76 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -541,6 +541,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; + case MAIN_STATE_OFFBOARD: + /* require offboard control, otherwise stay where you are */ + if (status->offboard_control_signal_lost && !status->rc_signal_lost) { + status->failsafe = true; + + status->nav_state = NAVIGATION_STATE_POSCTL; + } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = NAVIGATION_STATE_OFFBOARD; + } default: break; } -- cgit v1.2.3 From c99683752ccb62c9654b390c8761337f291c58e9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 21 Jun 2014 12:25:11 +0200 Subject: navigator: added missing offboard state --- src/modules/navigator/navigator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d762b8a9d..8ab50f0e1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -363,6 +363,7 @@ Navigator::task_main() break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: + case NAVIGATION_STATE_OFFBOARD: default: _navigation_mode = nullptr; _is_in_loiter = false; -- cgit v1.2.3 From f5699e6490a136f4344bfab10b72a9cdd4860569 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 21 Jun 2014 12:27:11 +0200 Subject: mavlink: always copy offboard setpoints, not just when control mode changed, also bugfixes in orb calls --- src/modules/mavlink/mavlink_receiver.cpp | 123 +++++++++++++++---------------- 1 file changed, 61 insertions(+), 62 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 80404a46a..aaae0933e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -407,11 +407,10 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message /*Temporary hack to use set_quad_swarm_roll_pitch_yaw_thrust msg for position or velocity */ /* Converts INT16 centimeters to float meters */ - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 100.0f; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 100.0f; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 100.0f; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 100.0f; - + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 1000.0f; } if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { @@ -434,83 +433,83 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message orb_check(_control_mode_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } - if (_control_mode.flag_control_offboard_enabled) { - if (_control_mode.flag_control_position_enabled) { - // TODO Use something else then quad_swarm_roll_pitch_yaw_thrust - struct vehicle_local_position_setpoint_s loc_pos_sp; - memset(&loc_pos_sp, 0, sizeof(loc_pos_sp)); - - loc_pos_sp.x = offboard_control_sp.p1; - loc_pos_sp.y = offboard_control_sp.p2; - loc_pos_sp.yaw = offboard_control_sp.p3; - loc_pos_sp.z = -offboard_control_sp.p4; + if (_control_mode.flag_control_offboard_enabled) { + if (_control_mode.flag_control_position_enabled) { + // TODO Use something else then quad_swarm_roll_pitch_yaw_thrust + struct vehicle_local_position_setpoint_s loc_pos_sp; + memset(&loc_pos_sp, 0, sizeof(loc_pos_sp)); - if (_local_pos_sp_pub < 0) { - _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub); + loc_pos_sp.x = offboard_control_sp.p1; + loc_pos_sp.y = offboard_control_sp.p2; + loc_pos_sp.yaw = offboard_control_sp.p3; + loc_pos_sp.z = -offboard_control_sp.p4; - } else { - orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp); - } + if (_local_pos_sp_pub < 0) { + _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &loc_pos_sp); - } else if (_control_mode.flag_control_velocity_enabled) { - /* velocity control */ - struct vehicle_global_velocity_setpoint_s global_vel_sp; - memset(&global_vel_sp, 0, sizeof(&global_vel_sp)); + } else { + orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp); + } - global_vel_sp.vx = offboard_control_sp.p1; - global_vel_sp.vy = offboard_control_sp.p2; - global_vel_sp.vz = offboard_control_sp.p3; + } else if (_control_mode.flag_control_velocity_enabled) { + /* velocity control */ + struct vehicle_global_velocity_setpoint_s global_vel_sp; + memset(&global_vel_sp, 0, sizeof(&global_vel_sp)); - if (_global_vel_sp_pub < 0) { - _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp_pub); + global_vel_sp.vx = offboard_control_sp.p1; + global_vel_sp.vy = offboard_control_sp.p2; + global_vel_sp.vz = offboard_control_sp.p3; - } else { - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp); - } + if (_global_vel_sp_pub < 0) { + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); - } else if (_control_mode.flag_control_attitude_enabled) { - /* attitude control */ - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); + } else { + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp); + } - att_sp.roll_body = offboard_control_sp.p1; - att_sp.pitch_body = offboard_control_sp.p2; - att_sp.yaw_body = offboard_control_sp.p3; - att_sp.thrust = offboard_control_sp.p4; + } else if (_control_mode.flag_control_attitude_enabled) { + /* attitude control */ + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); - att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = offboard_control_sp.p1; + att_sp.pitch_body = offboard_control_sp.p2; + att_sp.yaw_body = offboard_control_sp.p3; + att_sp.thrust = offboard_control_sp.p4; - if (_att_sp_pub < 0) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + att_sp.timestamp = hrt_absolute_time(); - } else { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); - } + if (_att_sp_pub < 0) { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - } else if (_control_mode.flag_control_rates_enabled) { - /* rates control */ - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); + } else { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + } - rates_sp.roll = offboard_control_sp.p1; - rates_sp.pitch = offboard_control_sp.p2; - rates_sp.yaw = offboard_control_sp.p3; - rates_sp.thrust = offboard_control_sp.p4; + } else if (_control_mode.flag_control_rates_enabled) { + /* rates control */ + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); - rates_sp.timestamp = hrt_absolute_time(); + rates_sp.roll = offboard_control_sp.p1; + rates_sp.pitch = offboard_control_sp.p2; + rates_sp.yaw = offboard_control_sp.p3; + rates_sp.thrust = offboard_control_sp.p4; - if (_rates_sp_pub < 0) { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + rates_sp.timestamp = hrt_absolute_time(); - } else { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); - } + if (_rates_sp_pub < 0) { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); } else { - /* direct control */ - // TODO + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); } + + } else { + /* direct control */ + // TODO } } } -- cgit v1.2.3 From 993e4d41d8165e27f03e51af5b809da6fd3a7509 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 21 Jun 2014 12:27:32 +0200 Subject: sensors: missing param for offboard switch --- src/modules/sensors/sensor_params.c | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 65e2376ce..dad81d8e1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -790,3 +790,20 @@ PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f); * */ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f); + + +/** + * Threshold for selecting offboard mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel Date: Sat, 21 Jun 2014 12:28:08 +0200 Subject: mc_pos_control: added missing subscribe --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a4c871d95..e020b2224 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -134,7 +134,7 @@ private: struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ - + struct { param_t thr_min; @@ -533,6 +533,8 @@ MulticopterPositionControl::task_main() _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + _global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); + parameters_update(true); @@ -699,7 +701,7 @@ MulticopterPositionControl::task_main() reset_pos_sp(); reset_alt_sp(); } - + } else { /* AUTO */ bool updated; -- cgit v1.2.3 From 7992a063c9f9b2cf7fbb49dcd5d19c60f15d0bfc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:34:34 +0200 Subject: navigator: added offboard mode --- src/modules/navigator/module.mk | 1 + src/modules/navigator/navigator.h | 8 +- src/modules/navigator/navigator_main.cpp | 7 ++ src/modules/navigator/offboard.cpp | 124 +++++++++++++++++++++++++++++++ src/modules/navigator/offboard.h | 82 ++++++++++++++++++++ 5 files changed, 221 insertions(+), 1 deletion(-) create mode 100644 src/modules/navigator/offboard.cpp create mode 100644 src/modules/navigator/offboard.h diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index a1e109030..637eaae59 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -46,6 +46,7 @@ SRCS = navigator_main.cpp \ loiter.cpp \ rtl.cpp \ rtl_params.c \ + offboard.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index dadd15527..e6ff213bd 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -56,13 +56,14 @@ #include "mission.h" #include "loiter.h" #include "rtl.h" +#include "offboard.h" #include "geofence.h" /** * Number of navigation modes that need on_active/on_inactive calls * Currently: mission, loiter, and rtl */ -#define NAVIGATOR_MODE_ARRAY_SIZE 3 +#define NAVIGATOR_MODE_ARRAY_SIZE 4 class Navigator : public control::SuperBlock { @@ -108,10 +109,13 @@ public: * Getters */ struct vehicle_status_s* get_vstatus() { return &_vstatus; } + struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } + int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } + int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } Geofence& get_geofence() { return _geofence; } bool get_is_in_loiter() { return _is_in_loiter; } float get_loiter_radius() { return _param_loiter_radius.get(); } @@ -129,6 +133,7 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _offboard_control_sp_sub; /*** offboard control subscription */ int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ @@ -158,6 +163,7 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ + Offboard _offboard; /**< class that handles offboard */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8ab50f0e1..8ba835a87 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include @@ -100,6 +101,7 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), + _offboard_control_sp_sub(-1), _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), @@ -121,6 +123,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), + _offboard(this, "OFF"), _update_triplet(false), _param_loiter_radius(this, "LOITER_RAD"), _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") @@ -129,6 +132,7 @@ Navigator::Navigator() : _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; + _navigation_mode_array[3] = &_offboard; updateParams(); } @@ -241,6 +245,7 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); + _offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); @@ -364,6 +369,8 @@ Navigator::task_main() case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: + _navigation_mode = &_offboard; + break; default: _navigation_mode = nullptr; _is_in_loiter = false; diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp new file mode 100644 index 000000000..b0cbb1aa1 --- /dev/null +++ b/src/modules/navigator/offboard.cpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file offboard.cpp + * + * Helper class for offboard commands + * + * @author Julian Oes + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "navigator.h" +#include "offboard.h" + +Offboard::Offboard(Navigator *navigator, const char *name) : + NavigatorMode(navigator, name), + _offboard_control_sp({0}) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +Offboard::~Offboard() +{ +} + +bool +Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + bool updated; + orb_check(_navigator->get_offboard_control_sp_sub(), &updated); + if (updated) { + update_offboard_control_setpoint(); + } + + bool changed = false; + + /* copy offboard setpoints to the corresponding topics */ + if (_navigator->get_control_mode()->flag_control_position_enabled + && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { + /* position control */ + pos_sp_triplet->current.x = _offboard_control_sp.p1; + pos_sp_triplet->current.y = _offboard_control_sp.p2; + pos_sp_triplet->current.yaw = _offboard_control_sp.p3; + pos_sp_triplet->current.z = -_offboard_control_sp.p4; + + pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.position_valid = true; + + changed = true; + + } else if (_navigator->get_control_mode()->flag_control_velocity_enabled + && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) { + /* velocity control */ + pos_sp_triplet->current.vx = _offboard_control_sp.p2; + pos_sp_triplet->current.vy = _offboard_control_sp.p1; + pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3*10.0f; + pos_sp_triplet->current.vz = _offboard_control_sp.p4; + + pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.velocity_valid = true; + + changed = true; + } + + return changed; +} + +void +Offboard::on_inactive() +{ +} + +void +Offboard::update_offboard_control_setpoint() +{ + orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp); + +} diff --git a/src/modules/navigator/offboard.h b/src/modules/navigator/offboard.h new file mode 100644 index 000000000..4d415dddb --- /dev/null +++ b/src/modules/navigator/offboard.h @@ -0,0 +1,82 @@ +/*************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file offboard.h + * + * Helper class for offboard commands + * + * @author Julian Oes + */ + +#ifndef NAVIGATOR_OFFBOARD_H +#define NAVIGATOR_OFFBOARD_H + +#include +#include + +#include +#include + +#include "navigator_mode.h" + +class Navigator; + +class Offboard : public NavigatorMode +{ +public: + /** + * Constructor + */ + Offboard(Navigator *navigator, const char *name); + + /** + * Destructor + */ + ~Offboard(); + + /** + * This function is called while the mode is inactive + */ + virtual void on_inactive(); + + /** + * This function is called while the mode is active + */ + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); +private: + void update_offboard_control_setpoint(); + + struct offboard_control_setpoint_s _offboard_control_sp; +}; + +#endif -- cgit v1.2.3 From a122e88893f4b983a6e8e2e2838f9a16f055c8d1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:35:02 +0200 Subject: commander: hack to get velocity offboard control working --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0c9ce4418..e4d132c40 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1776,9 +1776,9 @@ set_control_mode() case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */ control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = false; + control_mode.flag_control_position_enabled = true; /* XXX: hack for now */ control_mode.flag_control_velocity_enabled = true; break; case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: -- cgit v1.2.3 From 4c4d14347efe9d12712399a1a5eb0140fdd113fd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:43:29 +0200 Subject: mavlink: only save a offboard_control, don't publish to setpoint topics (let the navigator do this) --- src/modules/mavlink/mavlink_receiver.cpp | 154 +++---------------------------- 1 file changed, 11 insertions(+), 143 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index aaae0933e..c4955f1b3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -355,70 +355,22 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) void MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control); - if (mavlink_system.sysid < 4) { + /* Only accept system IDs from 1 to 4 + * TODO: check for own system ID */ + if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) { struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - enum OFFBOARD_CONTROL_MODE ml_mode = OFFBOARD_CONTROL_MODE_DIRECT; - bool ml_armed = false; + /* Convert values * 1000 back */ + offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; - switch (quad_motors_setpoint.mode) { - case 0: - break; - - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - ml_armed = true; - - break; - - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - ml_armed = true; - - break; - default: - break; - } - - if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES || ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - - } else if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { - - /*Temporary hack to use set_quad_swarm_roll_pitch_yaw_thrust msg for position or velocity */ - /* Converts INT16 centimeters to float meters */ - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 1000.0f; - } - - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { - ml_armed = false; - } - - offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = ml_mode; + offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; offboard_control_sp.timestamp = hrt_absolute_time(); @@ -428,90 +380,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } else { orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } - - bool updated; - orb_check(_control_mode_sub, &updated); - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); - } - - if (_control_mode.flag_control_offboard_enabled) { - if (_control_mode.flag_control_position_enabled) { - // TODO Use something else then quad_swarm_roll_pitch_yaw_thrust - struct vehicle_local_position_setpoint_s loc_pos_sp; - memset(&loc_pos_sp, 0, sizeof(loc_pos_sp)); - - loc_pos_sp.x = offboard_control_sp.p1; - loc_pos_sp.y = offboard_control_sp.p2; - loc_pos_sp.yaw = offboard_control_sp.p3; - loc_pos_sp.z = -offboard_control_sp.p4; - - if (_local_pos_sp_pub < 0) { - _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &loc_pos_sp); - - } else { - orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp); - } - - } else if (_control_mode.flag_control_velocity_enabled) { - /* velocity control */ - struct vehicle_global_velocity_setpoint_s global_vel_sp; - memset(&global_vel_sp, 0, sizeof(&global_vel_sp)); - - global_vel_sp.vx = offboard_control_sp.p1; - global_vel_sp.vy = offboard_control_sp.p2; - global_vel_sp.vz = offboard_control_sp.p3; - - if (_global_vel_sp_pub < 0) { - _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); - - } else { - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp); - } - - } else if (_control_mode.flag_control_attitude_enabled) { - /* attitude control */ - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - - att_sp.roll_body = offboard_control_sp.p1; - att_sp.pitch_body = offboard_control_sp.p2; - att_sp.yaw_body = offboard_control_sp.p3; - att_sp.thrust = offboard_control_sp.p4; - - att_sp.timestamp = hrt_absolute_time(); - - if (_att_sp_pub < 0) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - } else { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); - } - - } else if (_control_mode.flag_control_rates_enabled) { - /* rates control */ - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - - rates_sp.roll = offboard_control_sp.p1; - rates_sp.pitch = offboard_control_sp.p2; - rates_sp.yaw = offboard_control_sp.p3; - rates_sp.thrust = offboard_control_sp.p4; - - rates_sp.timestamp = hrt_absolute_time(); - - if (_rates_sp_pub < 0) { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - - } else { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); - } - - } else { - /* direct control */ - // TODO - } - } } } -- cgit v1.2.3 From df9381636a2fa96e1bc02b4244273ec6b7f11aa9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:44:05 +0200 Subject: offboard_control_setpoint: armed does not make sense there --- src/modules/uORB/topics/offboard_control_setpoint.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 68d3e494b..d7b131e3c 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -69,7 +69,7 @@ struct offboard_control_setpoint_s { uint64_t timestamp; enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - bool armed; /**< Armed flag set, yes / no */ + float p1; /**< ailerons roll / roll rate input */ float p2; /**< elevator / pitch / pitch rate */ float p3; /**< rudder / yaw rate / yaw */ -- cgit v1.2.3 From 0b172d662a8926dbe1c8a0c59766f7c63bb8f4fe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:44:38 +0200 Subject: position_setpoint_triplet: add local position and velocity setpoints --- src/modules/uORB/topics/position_setpoint_triplet.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index ce42035ba..673c0e491 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -60,16 +60,26 @@ enum SETPOINT_TYPE SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ + SETPOINT_TYPE_OFFBOARD, /**< setpoint set by offboard */ }; struct position_setpoint_s { bool valid; /**< true if setpoint is valid */ enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ + float x; /**< local position setpoint in m in NED */ + float y; /**< local position setpoint in m in NED */ + float z; /**< local position setpoint in m in NED */ + bool position_valid; /**< true if local position setpoint valid */ + float vx; /**< local velocity setpoint in m in NED */ + float vy; /**< local velocity setpoint in m in NED */ + float vz; /**< local velocity setpoint in m in NED */ + bool velocity_valid; /**< true if local velocity setpoint valid */ double lat; /**< latitude, in deg */ double lon; /**< longitude, in deg */ float alt; /**< altitude AMSL, in m */ float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */ float loiter_radius; /**< loiter radius (only for fixed wing), in m */ int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ -- cgit v1.2.3 From ee872d91b05bf47d44dae4f5cc07be1631db91c1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:45:48 +0200 Subject: mc_pos_control: read velocity setpoints form offboard control and do position offset control --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 88 +++++++++++++++------- 1 file changed, 59 insertions(+), 29 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index e020b2224..6541788f6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -556,6 +556,9 @@ MulticopterPositionControl::task_main() math::Vector<3> sp_move_rate; sp_move_rate.zero(); + + float yaw_sp_move_rate; + math::Vector<3> thrust_int; thrust_int.zero(); math::Matrix<3, 3> R; @@ -674,28 +677,73 @@ MulticopterPositionControl::task_main() } else if (_control_mode.flag_control_offboard_enabled) { /* Offboard control */ bool updated; - orb_check(_local_pos_sp_sub, &updated); + orb_check(_pos_sp_triplet_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp); + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); } - if (isfinite(_local_pos_sp.x) && isfinite(_local_pos_sp.y) && isfinite(_local_pos_sp.z) && isfinite(_local_pos_sp.yaw)) { - /* If manual control overides offboard, cancel the offboard setpoint and stay at current position */ - _reset_pos_sp = true; - _reset_alt_sp = true; + if (_pos_sp_triplet.current.valid) { + + if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) { + + _pos_sp(0) = _pos_sp_triplet.current.x; + _pos_sp(1) = _pos_sp_triplet.current.y; + _pos_sp(2) = _pos_sp_triplet.current.z; + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + + } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + /* move position setpoint with roll/pitch stick */ + sp_move_rate(0) = _pos_sp_triplet.current.vx; + sp_move_rate(1) = _pos_sp_triplet.current.vy; + yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed; + _att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt; + } + + if (_control_mode.flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + + /* move altitude setpoint with throttle stick */ + sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);; + } + + /* limit setpoint move rate */ + float sp_move_norm = sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + sp_move_rate /= sp_move_norm; + } + + /* scale to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); + + /* move position setpoint */ + _pos_sp += sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); - /* Make sure position control is selected i.e. not only velocity control */ if (_control_mode.flag_control_position_enabled) { - _pos_sp(0) = _local_pos_sp.x; - _pos_sp(1) = _local_pos_sp.y; + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); } if (_control_mode.flag_control_altitude_enabled) { - _pos_sp(2) = _local_pos_sp.z; + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); } - _att_sp.yaw_body = _local_pos_sp.yaw; + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } } else { reset_pos_sp(); @@ -792,24 +840,6 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _params.land_speed; } - /* Offboard velocity control mode */ - if (_control_mode.flag_control_offboard_enabled) { - bool updated; - orb_check(_global_vel_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_sub, &_global_vel_sp); - } - - if (!_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_climb_rate_enabled) { - _vel_sp(2) = _global_vel_sp.vz; - } - - if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) { - _vel_sp(0) = _global_vel_sp.vx; - _vel_sp(1) = _global_vel_sp.vy; - } - } if (!_control_mode.flag_control_manual_enabled) { /* limit 3D speed only in non-manual modes */ -- cgit v1.2.3 From 615277d346e60afd74a9223d5b5f8a20b7b47ece Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Jun 2014 13:29:04 +0200 Subject: Better comments in estimator --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 1a2151017..c122b3e51 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2390,6 +2390,8 @@ int AttPosEKF::CheckAndBound() ret = 5; } + // The excessive covariance detection already + // reset the filter. Just need to report here. if (current_ekf_state.covariancesExcessive) { FillErrorReport(&last_ekf_error); current_ekf_state.covariancesExcessive = false; -- cgit v1.2.3 From 3195eb100516b7a4aabadd3e2640434678dbc7ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Jun 2014 13:35:23 +0200 Subject: estimator: Remove bogus timeout flag, do not reset states not in need of a reset. Do not alter baro offset or GPS positions. --- .../ekf_att_pos_estimator_main.cpp | 21 +++------------------ 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5b0e7acf2..4f6f65393 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -208,7 +208,6 @@ private: perf_counter_t _perf_airspeed; ///GetLastErrorState(&ekf_report); - // set sensors to de-initialized state - _gyro_valid = false; - _accel_valid = false; - _mag_valid = false; - - _baro_init = false; - _gps_initialized = false; - _initialized = false; - _last_sensor_timestamp = hrt_absolute_time(); - _last_run = _last_sensor_timestamp; - - _ekf->dtIMU = 0.01f; - } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); } @@ -665,6 +650,7 @@ FixedwingEstimator::check_filter_state() rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); + rep.timeout_flags |= (((uint8_t)(check == 2)) << 3); // IMU timeout if (_debug > 10) { @@ -680,7 +666,8 @@ FixedwingEstimator::check_filter_state() warnx("timeout: %s%s%s", ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), - ((rep.timeout_flags & (1 << 2)) ? "HGT " : "")); + ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), + ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); } } @@ -1263,8 +1250,6 @@ FixedwingEstimator::task_main() dt = 0.0f; } - _initialized = true; - // Fuse GPS Measurements if (newDataGps && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED -- cgit v1.2.3 From 93887f9f28d978290e6698b7f2b7e1e9b60bec13 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 14:04:50 +0200 Subject: navigator: remove hacked scale factor for offboard control --- src/modules/navigator/offboard.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index b0cbb1aa1..ef7d11a03 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -98,7 +98,7 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) /* velocity control */ pos_sp_triplet->current.vx = _offboard_control_sp.p2; pos_sp_triplet->current.vy = _offboard_control_sp.p1; - pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3*10.0f; + pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; pos_sp_triplet->current.vz = _offboard_control_sp.p4; pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; -- cgit v1.2.3 From ee6e0000980f2127913dfec73b1981b559f85877 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 14:08:13 +0200 Subject: mavlink: fix comment in swarm message receiver --- src/modules/mavlink/mavlink_receiver.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c4955f1b3..c96bd9273 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -358,8 +358,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control; mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control); - /* Only accept system IDs from 1 to 4 - * TODO: check for own system ID */ + /* Only accept system IDs from 1 to 4 */ if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) { struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); -- cgit v1.2.3 From 6951e1c95e3b58c35a031aff92b228b3f651a94c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Jun 2014 15:00:48 +0200 Subject: estimator lib: Improve error reporting --- .../ekf_att_pos_estimator/estimator_23states.cpp | 117 ++++++++++++++------- .../ekf_att_pos_estimator/estimator_23states.h | 5 +- .../ekf_att_pos_estimator/estimator_utilities.h | 3 + 3 files changed, 82 insertions(+), 43 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index c122b3e51..4c503b5d4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -46,6 +46,7 @@ AttPosEKF::AttPosEKF() lastReset = 0; memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); ZeroVariables(); InitialiseParameters(); } @@ -2097,7 +2098,8 @@ void AttPosEKF::ForceSymmetry() if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) || (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) { - last_ekf_error.covariancesExcessive = true; + current_ekf_state.covariancesExcessive = true; + current_ekf_state.error |= true; InitializeDynamic(velNED, magDeclination); return; } @@ -2129,6 +2131,8 @@ bool AttPosEKF::GyroOffsetsDiverged() bool diverged = (delta_len_scaled > 1.0f); lastGyroOffset = current_bias; + current_ekf_state.error |= diverged; + current_ekf_state.gyroOffsetsExcessive = diverged; return diverged; } @@ -2148,7 +2152,12 @@ bool AttPosEKF::VelNEDDiverged() Vector3f delta = current_vel - gps_vel; float delta_len = delta.length(); - return (delta_len > 20.0f); + bool excessive = (delta_len > 20.0f); + + current_ekf_state.error |= excessive; + current_ekf_state.velOffsetExcessive = excessive; + + return excessive; } bool AttPosEKF::FilterHealthy() @@ -2215,43 +2224,26 @@ void AttPosEKF::ResetVelocity(void) } } - -void AttPosEKF::FillErrorReport(struct ekf_status_report *err) -{ - for (unsigned i = 0; i < n_states; i++) - { - err->states[i] = states[i]; - } - err->n_states = n_states; - - err->velHealth = current_ekf_state.velHealth; - err->posHealth = current_ekf_state.posHealth; - err->hgtHealth = current_ekf_state.hgtHealth; - err->velTimeout = current_ekf_state.velTimeout; - err->posTimeout = current_ekf_state.posTimeout; - err->hgtTimeout = current_ekf_state.hgtTimeout; -} - -bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { +bool AttPosEKF::StatesNaN() { bool err = false; // check all integrators if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { - err_report->angNaN = true; + current_ekf_state.angNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { - err_report->angNaN = true; + current_ekf_state.angNaN = true; ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { - err_report->summedDelVelNaN = true; + current_ekf_state.summedDelVelNaN = true; ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); err = true; goto out; @@ -2262,7 +2254,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { for (unsigned j = 0; j < n_states; j++) { if (!isfinite(KH[i][j])) { - err_report->KHNaN = true; + current_ekf_state.KHNaN = true; err = true; ekf_debug("KH NaN"); goto out; @@ -2270,7 +2262,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(KHP[i][j])) { - err_report->KHPNaN = true; + current_ekf_state.KHPNaN = true; err = true; ekf_debug("KHP NaN"); goto out; @@ -2278,7 +2270,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(P[i][j])) { - err_report->covarianceNaN = true; + current_ekf_state.covarianceNaN = true; err = true; ekf_debug("P NaN"); } // covariance matrix @@ -2286,7 +2278,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(Kfusion[i])) { - err_report->kalmanGainsNaN = true; + current_ekf_state.kalmanGainsNaN = true; ekf_debug("Kfusion NaN"); err = true; goto out; @@ -2294,7 +2286,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(states[i])) { - err_report->statesNaN = true; + current_ekf_state.statesNaN = true; ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); err = true; goto out; @@ -2303,7 +2295,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { out: if (err) { - FillErrorReport(err_report); + current_ekf_state.error |= true; } return err; @@ -2319,7 +2311,7 @@ out: * updated, but before any of the fusion steps are * executed. */ -int AttPosEKF::CheckAndBound() +int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) { // Store the old filter state @@ -2341,9 +2333,10 @@ int AttPosEKF::CheckAndBound() OnGroundCheck(); // Reset the filter if the states went NaN - if (StatesNaN(&last_ekf_error)) { + if (StatesNaN()) { ekf_debug("re-initializing dynamic"); + // Reset and fill error report InitializeDynamic(velNED, magDeclination); ret = 1; @@ -2351,19 +2344,29 @@ int AttPosEKF::CheckAndBound() // Reset the filter if the IMU data is too old if (dtIMU > 0.3f) { - FillErrorReport(&last_ekf_error); + + current_ekf_state.imuTimeout = true; + + // Fill error report + GetFilterState(&last_ekf_error); + ResetVelocity(); ResetPosition(); ResetHeight(); ResetStoredStates(); + // Timeout cleared with this reset + current_ekf_state.imuTimeout = false; + // that's all we can do here, return ret = 2; } // Check if we switched between states if (currStaticMode != staticMode) { - FillErrorReport(&last_ekf_error); + // Fill error report, but not setting error flag + GetFilterState(&last_ekf_error); + ResetVelocity(); ResetPosition(); ResetHeight(); @@ -2374,7 +2377,8 @@ int AttPosEKF::CheckAndBound() // Reset the filter if gyro offsets are excessive if (GyroOffsetsDiverged()) { - FillErrorReport(&last_ekf_error); + + // Reset and fill error report InitializeDynamic(velNED, magDeclination); // that's all we can do here, return @@ -2383,7 +2387,8 @@ int AttPosEKF::CheckAndBound() // Reset the filter if it diverges too far from GPS if (VelNEDDiverged()) { - FillErrorReport(&last_ekf_error); + + // Reset and fill error report InitializeDynamic(velNED, magDeclination); // that's all we can do here, return @@ -2392,15 +2397,16 @@ int AttPosEKF::CheckAndBound() // The excessive covariance detection already // reset the filter. Just need to report here. - if (current_ekf_state.covariancesExcessive) { - FillErrorReport(&last_ekf_error); - current_ekf_state.covariancesExcessive = false; + if (last_ekf_error.covariancesExcessive) { ret = 6; } if (ret) { ekfDiverged = true; lastReset = millis(); + + // This reads the last error and clears it + GetLastErrorState(last_error); } return ret; @@ -2454,8 +2460,31 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { + if (current_ekf_state.error) { + GetFilterState(&last_ekf_error); + } + ZeroVariables(); + // Reset error states + current_ekf_state.error = false; + current_ekf_state.angNaN = false; + current_ekf_state.summedDelVelNaN = false; + current_ekf_state.KHNaN = false; + current_ekf_state.KHPNaN = false; + current_ekf_state.PNaN = false; + current_ekf_state.covarianceNaN = false; + current_ekf_state.kalmanGainsNaN = false; + current_ekf_state.statesNaN = false; + + current_ekf_state.velHealth = true; + //current_ekf_state.posHealth = ?; + //current_ekf_state.hgtHealth = ?; + + current_ekf_state.velTimeout = false; + //current_ekf_state.posTimeout = ?; + //current_ekf_state.hgtTimeout = ?; + // Fill variables with valid data velNED[0] = initvelNED[0]; velNED[1] = initvelNED[1]; @@ -2582,7 +2611,7 @@ void AttPosEKF::ZeroVariables() } -void AttPosEKF::GetFilterState(struct ekf_status_report *state) +void AttPosEKF::GetFilterState(struct ekf_status_report *err) { // Copy states @@ -2591,10 +2620,18 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *state) } current_ekf_state.n_states = n_states; - memcpy(state, ¤t_ekf_state, sizeof(*state)); + memcpy(err, ¤t_ekf_state, sizeof(*err)); + + // err->velHealth = current_ekf_state.velHealth; + // err->posHealth = current_ekf_state.posHealth; + // err->hgtHealth = current_ekf_state.hgtHealth; + // err->velTimeout = current_ekf_state.velTimeout; + // err->posTimeout = current_ekf_state.posTimeout; + // err->hgtTimeout = current_ekf_state.hgtTimeout; } void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) { memcpy(last_error, &last_ekf_error, sizeof(*last_error)); + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 10a646025..5a1f5125a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -266,7 +266,7 @@ void ConstrainStates(); void ForceSymmetry(); -int CheckAndBound(); +int CheckAndBound(struct ekf_status_report *last_error); void ResetPosition(); @@ -278,8 +278,7 @@ void GetFilterState(struct ekf_status_report *state); void GetLastErrorState(struct ekf_status_report *last_error); -bool StatesNaN(struct ekf_status_report *err_report); -void FillErrorReport(struct ekf_status_report *err); +bool StatesNaN(); void InitializeDynamic(float (&initvelNED)[3], float declination); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index e5f76d7cd..97f22b453 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -53,12 +53,14 @@ enum GPS_FIX { }; struct ekf_status_report { + bool error; bool velHealth; bool posHealth; bool hgtHealth; bool velTimeout; bool posTimeout; bool hgtTimeout; + bool imuTimeout; uint32_t velFailTime; uint32_t posFailTime; uint32_t hgtFailTime; @@ -74,6 +76,7 @@ struct ekf_status_report { bool statesNaN; bool gyroOffsetsExcessive; bool covariancesExcessive; + bool velOffsetExcessive; }; void ekf_debug(const char *fmt, ...); \ No newline at end of file -- cgit v1.2.3 From efb20d50bdc08e737635fd3aff0f2caa8872c6cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Jun 2014 15:01:19 +0200 Subject: estimator: Use improved error reporting API --- .../ekf_att_pos_estimator_main.cpp | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 4f6f65393..3a2b7920d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -554,9 +554,12 @@ FixedwingEstimator::check_filter_state() /* * CHECK IF THE INPUT DATA IS SANE */ - int check = _ekf->CheckAndBound(); - const char* ekfname = "[ekf] "; + struct ekf_status_report ekf_report; + + int check = _ekf->CheckAndBound(&ekf_report); + + const char* ekfname = "#audio: "; switch (check) { case 0: @@ -592,7 +595,7 @@ FixedwingEstimator::check_filter_state() } case 5: { - const char* str = "diverged too far from GPS"; + const char* str = "GPS velocity divergence"; warnx("%s", str); mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); break; @@ -616,16 +619,12 @@ FixedwingEstimator::check_filter_state() struct estimator_status_report rep; memset(&rep, 0, sizeof(rep)); - struct ekf_status_report ekf_report; - - // If non-zero, we got a filter reset - if (check > 0 && check != 3) { + // If error flag is set, we got a filter reset + if (check && ekf_report.error) { // Count the reset condition perf_count(_perf_reset); - _ekf->GetLastErrorState(&ekf_report); - } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); } @@ -645,12 +644,12 @@ FixedwingEstimator::check_filter_state() rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); - rep.health_flags |= ((!(check == 4)) << 3); + rep.health_flags |= (((uint8_t)ekf_report.gyroOffsetsExcessive) << 3); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); - rep.timeout_flags |= (((uint8_t)(check == 2)) << 3); // IMU timeout + rep.timeout_flags |= (((uint8_t)ekf_report.imuTimeout) << 3); if (_debug > 10) { -- cgit v1.2.3 From 12c2802fa396d40fe9fc45f384352d9eff23ced0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Jun 2014 16:23:53 +0200 Subject: estimator: Use the right perf counters to measure the quantities we are interested in --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3a2b7920d..f1ae3f5e5 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -370,12 +370,12 @@ FixedwingEstimator::FixedwingEstimator() : _baro_gps_offset(0.0f), /* performance counters */ - _loop_perf(perf_alloc(PC_COUNT, "ekf_att_pos_estimator")), - _perf_gyro(perf_alloc(PC_COUNT, "ekf_att_pos_gyro_upd")), - _perf_mag(perf_alloc(PC_COUNT, "ekf_att_pos_mag_upd")), - _perf_gps(perf_alloc(PC_COUNT, "ekf_att_pos_gps_upd")), - _perf_baro(perf_alloc(PC_COUNT, "ekf_att_pos_baro_upd")), - _perf_airspeed(perf_alloc(PC_COUNT, "ekf_att_pos_aspd_upd")), + _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), + _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), + _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), /* states */ -- cgit v1.2.3 From d45cc69d1d62a2ccac00757ab02a731fca6d6ece Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 27 Jun 2014 21:44:21 +0200 Subject: mtecs/wind: store wind variance --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 93ed18b8d..272ad5344 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1345,8 +1345,8 @@ FixedwingEstimator::task_main() _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = 0.0f; // XXX get form filter - _wind.covariance_east = 0.0f; + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; /* lazily publish the wind estimate only once available */ if (_wind_pub > 0) { -- cgit v1.2.3 From 456e628e129b446d18246ab8ad312a15beea5996 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 28 Jun 2014 00:54:27 +0200 Subject: navigator: NavigatorMode and MissionBlock API cleanup --- src/modules/navigator/loiter.cpp | 18 ++++++--- src/modules/navigator/loiter.h | 14 +------ src/modules/navigator/mission.cpp | 14 +++++-- src/modules/navigator/mission.h | 14 +------ src/modules/navigator/mission_block.cpp | 65 ++++++++++++++++++-------------- src/modules/navigator/mission_block.h | 14 +++---- src/modules/navigator/navigator_main.cpp | 6 +-- src/modules/navigator/navigator_mode.cpp | 34 +++++++++++++++-- src/modules/navigator/navigator_mode.h | 12 +++++- src/modules/navigator/rtl.cpp | 62 ++++++++++++++---------------- src/modules/navigator/rtl.h | 20 ++-------- 11 files changed, 144 insertions(+), 129 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 542483fb1..5e7067b0e 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -65,14 +65,22 @@ Loiter::~Loiter() { } -bool -Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Loiter::on_inactive() { - /* set loiter item, don't reuse an existing position setpoint */ - return set_loiter_item(pos_sp_triplet); } void -Loiter::on_inactive() +Loiter::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + set_loiter_item(pos_sp_triplet); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; +} + +bool +Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { + return false; } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 65ff5c31e..5ce86d2a7 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -50,24 +50,14 @@ class Loiter : public MissionBlock { public: - /** - * Constructor - */ Loiter(Navigator *navigator, const char *name); - /** - * Destructor - */ ~Loiter(); - /** - * This function is called while the mode is inactive - */ virtual void on_inactive(); - /** - * This function is called while the mode is active - */ + virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); }; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 72255103b..395669698 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -82,8 +82,6 @@ Mission::~Mission() void Mission::on_inactive() { - _first_run = true; - /* check anyway if missions have changed so that feedback to groundstation is given */ bool onboard_updated; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); @@ -98,6 +96,12 @@ Mission::on_inactive() } } +void +Mission::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + set_mission_items(pos_sp_triplet); +} + bool Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { @@ -117,10 +121,9 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) } /* reset mission items if needed */ - if (onboard_updated || offboard_updated || _first_run) { + if (onboard_updated || offboard_updated) { set_mission_items(pos_sp_triplet); updated = true; - _first_run = false; } /* lets check if we reached the current mission item */ @@ -255,6 +258,9 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); set_loiter_item(pos_sp_triplet); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; reset_mission_item_reached(); report_mission_finished(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6e4761946..38a4f7612 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -65,24 +65,14 @@ class Navigator; class Mission : public MissionBlock { public: - /** - * Constructor - */ Mission(Navigator *navigator, const char *name); - /** - * Destructor - */ virtual ~Mission(); - /** - * This function is called while the mode is inactive - */ virtual void on_inactive(); - /** - * This function is called while the mode is active - */ + virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); private: diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 9b8d3d9c7..acf3ad569 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -45,6 +45,7 @@ #include #include +#include #include @@ -207,36 +208,44 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_ } } -bool +void MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) { - /* don't change setpoint if 'can_loiter_at_sp' flag set */ - if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { - /* use current position */ - pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; - pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; - pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; - pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */ - - _navigator->set_can_loiter_at_sp(true); - } + if (_navigator->get_vstatus()->condition_landed) { + /* landed, don't takeoff, but switch to IDLE mode */ + _mission_item.nav_cmd = NAV_CMD_IDLE; - if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER - || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius() - || pos_sp_triplet->current.loiter_direction != 1 - || pos_sp_triplet->previous.valid - || !pos_sp_triplet->current.valid - || pos_sp_triplet->next.valid) { - /* position setpoint triplet should be updated */ - pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; - pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius(); - pos_sp_triplet->current.loiter_direction = 1; - - pos_sp_triplet->previous.valid = false; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->next.valid = false; - return true; - } + _navigator->set_can_loiter_at_sp(false); + + mavlink_log_info(_navigator->get_mavlink_fd(), "landed, switch to IDLE"); + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - return false; + if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) { + /* use current position setpoint */ + _mission_item.lat = pos_sp_triplet->current.lat; + _mission_item.lon = pos_sp_triplet->current.lon; + _mission_item.altitude = pos_sp_triplet->current.alt; + + } else { + /* use current position */ + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = _navigator->get_global_position()->alt; + } + + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + mavlink_log_info(_navigator->get_mavlink_fd(), "switch to LOITER"); + } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index f99002752..4f79238f4 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -64,6 +64,7 @@ public: */ virtual ~MissionBlock(); +protected: /** * Check if mission item has been reached * @return true if successfully reached @@ -88,19 +89,16 @@ public: void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet); /** - * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position - * - * @param the position setpoint triplet to set - * @return true if setpoint has changed + * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position */ - bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet); + // TODO remove argument, get position setpoint from navigator, add to arguments pointer to mission item instead + void set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet); + mission_item_s _mission_item; + bool _mission_item_valid; bool _waypoint_position_reached; bool _waypoint_yaw_reached; hrt_abstime _time_first_inside_orbit; - - mission_item_s _mission_item; - bool _mission_item_valid; }; #endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 546602741..266114e38 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -371,11 +371,7 @@ Navigator::task_main() /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { - if (_navigation_mode == _navigation_mode_array[i]) { - _update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet); - } else { - _navigation_mode_array[i]->on_inactive(); - } + _update_triplet = _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i], &_pos_sp_triplet); } /* if nothing is running, set position setpoint triplet invalid */ diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 25e767c2b..6361ea9c8 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -33,9 +33,10 @@ /** * @file navigator_mode.cpp * - * Helper class for different modes in navigator + * Base class for different modes in navigator * * @author Julian Oes + * @author Anton Babushkin */ #include "navigator_mode.h" @@ -55,16 +56,41 @@ NavigatorMode::~NavigatorMode() { } +bool +NavigatorMode::run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet) { + if (active) { + if (_first_run) { + /* first run */ + _first_run = false; + on_activation(pos_sp_triplet); + return true; + + } else { + /* periodic updates when active */ + on_active(pos_sp_triplet); + } + + } else { + /* periodic updates when inactive */ + _first_run = true; + on_inactive(); + } +} + void NavigatorMode::on_inactive() { - _first_run = true; +} + +void +NavigatorMode::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + /* invalidate position setpoint by default */ + pos_sp_triplet->current.valid = false; } bool NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { - pos_sp_triplet->current.valid = false; - _first_run = false; return false; } diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index cbb53d91b..5c36af1fe 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -33,9 +33,10 @@ /** * @file navigator_mode.h * - * Helper class for different modes in navigator + * Base class for different modes in navigator * * @author Julian Oes + * @author Anton Babushkin */ #ifndef NAVIGATOR_MODE_H @@ -65,11 +66,18 @@ public: */ virtual ~NavigatorMode(); + bool run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet); + /** * This function is called while the mode is inactive */ virtual void on_inactive(); + /** + * This function is called one time when mode become active, poos_sp_triplet must be initialized here + */ + virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + /** * This function is called while the mode is active * @@ -80,6 +88,8 @@ public: protected: Navigator *_navigator; + +private: bool _first_run; }; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 043f773a4..7cf6bb1a8 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -75,53 +75,49 @@ RTL::~RTL() void RTL::on_inactive() { - _first_run = true; - /* reset RTL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { _rtl_state = RTL_STATE_NONE; } } -bool -RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +RTL::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) { - bool updated = false; - - if (_first_run) { - _first_run = false; - - /* decide where to enter the RTL procedure when we switch into it */ - if (_rtl_state == RTL_STATE_NONE) { - /* for safety reasons don't go into RTL if landed */ - if (_navigator->get_vstatus()->condition_landed) { - _rtl_state = RTL_STATE_LANDED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - - /* if lower than return altitude, climb up first */ - } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - + _param_return_alt.get()) { - _rtl_state = RTL_STATE_CLIMB; - - /* otherwise go straight to return */ - } else { - /* set altitude setpoint to current altitude */ - _rtl_state = RTL_STATE_RETURN; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_global_position()->alt; - } + /* decide where to enter the RTL procedure when we switch into it */ + if (_rtl_state == RTL_STATE_NONE) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_LANDED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + /* if lower than return altitude, climb up first */ + } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + /* otherwise go straight to return */ + } else { + /* set altitude setpoint to current altitude */ + _rtl_state = RTL_STATE_RETURN; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; } + } - set_rtl_item(pos_sp_triplet); - updated = true; + set_rtl_item(pos_sp_triplet); +} - } else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { +bool +RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { advance_rtl(); set_rtl_item(pos_sp_triplet); - updated = true; + return true; } - return updated; + return false; } void diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index b4b729e89..9c8b3fdfc 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -57,29 +57,15 @@ class Navigator; class RTL : public MissionBlock { public: - /** - * Constructor - */ RTL(Navigator *navigator, const char *name); - /** - * Destructor - */ ~RTL(); - /** - * This function is called while the mode is inactive - */ - void on_inactive(); + virtual void on_inactive(); - /** - * This function is called while the mode is active - * - * @param position setpoint triplet that needs to be set - * @return true if updated - */ - bool on_active(position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(position_setpoint_triplet_s *pos_sp_triplet); private: /** -- cgit v1.2.3 From a398d5cbcc1d39d83611543b787c5c8aecae10dd Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 27 Jun 2014 20:33:39 -0700 Subject: Support for List, Open, Read, Terminate commands Fixed various bugs. Flattened Mavlink::Session class while fixing bugs in this area. --- src/modules/mavlink/mavlink_ftp.cpp | 167 ++++++++++++++++------------------- src/modules/mavlink/mavlink_ftp.h | 33 +++---- src/modules/mavlink/mavlink_main.cpp | 54 ++++++----- src/modules/mavlink/mavlink_main.h | 3 + 4 files changed, 122 insertions(+), 135 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index bfee17342..f1436efb8 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -55,6 +55,11 @@ MavlinkFTP::MavlinkFTP() // initialise the request freelist dq_init(&_workFree); sem_init(&_lock, 0, 1); + + // initialize session list + for (size_t i=0; isession)) { - errorCode = kErrNoRequest; - } + errorCode = _workTerminate(req); break; case kCmdReset: - Session::reset(); + errorCode = _workReset(); break; case kCmdList: @@ -219,6 +222,12 @@ MavlinkFTP::_workList(Request *req) // no more entries? if (result == nullptr) { + if (hdr->offset != 0 && offset == 0) { + // User is requesting subsequent dir entries but there were none. This means the user asked + // to seek past EOF. + errorCode = kErrEOF; + } + // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that break; } @@ -256,20 +265,21 @@ MavlinkFTP::ErrorCode MavlinkFTP::_workOpen(Request *req, bool create) { auto hdr = req->header(); - - // allocate session ID - int session = Session::allocate(); - if (session < 0) { + + int session_index = _findUnusedSession(); + if (session_index < 0) { return kErrNoSession; } - // get the session to open the file - if (!Session::get(session)->open(req->dataAsCString(), create)) { + int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; + + int fd = ::open(req->dataAsCString(), oflag); + if (fd < 0) { return create ? kErrPerm : kErrNotFile; } + _session_fds[session_index] = fd; - // save the session ID in the reply - hdr->session = session; + hdr->session = session_index; hdr->size = 0; return kErrNone; @@ -280,29 +290,40 @@ MavlinkFTP::_workRead(Request *req) { auto hdr = req->header(); - // look up session - auto session = Session::get(hdr->session); - if (session == nullptr) { + int session_index = hdr->session; + + if (!_validSession(session_index)) { return kErrNoSession; + } + + // Seek to the specified position + printf("Seek %d\n", hdr->offset); + if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + return kErrEOF; } - - // read from file - int result = session->read(hdr->offset, &hdr->data[0], hdr->size); - - if (result < 0) { + + int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); + if (bytes_read < 0) { + // Negative return indicates error other than eof return kErrIO; } - hdr->size = result; + + printf("Read success %d\n", bytes_read); + hdr->size = bytes_read; + return kErrNone; } MavlinkFTP::ErrorCode MavlinkFTP::_workWrite(Request *req) { +#if 0 + // NYI: Coming soon auto hdr = req->header(); // look up session - auto session = Session::get(hdr->session); + auto session = getSession(hdr->session); if (session == nullptr) { return kErrNoSession; } @@ -317,6 +338,9 @@ MavlinkFTP::_workWrite(Request *req) hdr->size = result; return kErrNone; +#else + return kErrPerm; +#endif } MavlinkFTP::ErrorCode @@ -328,91 +352,52 @@ MavlinkFTP::_workRemove(Request *req) return kErrPerm; } -MavlinkFTP::Session MavlinkFTP::Session::_sessions[MavlinkFTP::Session::kMaxSession]; - -int -MavlinkFTP::Session::allocate() -{ - for (unsigned i = 0; i < kMaxSession; i++) { - if (_sessions[i]._fd < 0) { - return i; - } - } - return -1; -} - -MavlinkFTP::Session * -MavlinkFTP::Session::get(unsigned index) -{ - if ((index >= kMaxSession) || (_sessions[index]._fd < 0)) { - return nullptr; - } - return &_sessions[index]; -} - -void -MavlinkFTP::Session::terminate() +MavlinkFTP::ErrorCode +MavlinkFTP::_workTerminate(Request *req) { - // clean up aborted transfers? - if (_fd >= 0) { - close(_fd); - _fd = -1; - } -} - -bool -MavlinkFTP::Session::terminate(unsigned index) - { - Session *session = get(index); - - if (session == nullptr) { - return false; - } + auto hdr = req->header(); + + if (!_validSession(hdr->session)) { + return kErrNoSession; + } + + ::close(_session_fds[hdr->session]); - session->terminate(); - return true; + return kErrNone; } -void -MavlinkFTP::Session::reset() +MavlinkFTP::ErrorCode +MavlinkFTP::_workReset(void) { - for (unsigned i = 0; i < kMaxSession; i++) { - terminate(i); - } + for (size_t i=0; i= kMaxSession) || (_session_fds[index] < 0)) { return false; } return true; } int -MavlinkFTP::Session::read(off_t offset, uint8_t *buf, uint8_t count) +MavlinkFTP::_findUnusedSession(void) { - // can we seek to the location? - if (lseek(_fd, offset, SEEK_SET) < 0) { - return -1; - } - - return read(_fd, buf, count); -} - -int -MavlinkFTP::Session::append(off_t offset, uint8_t *buf, uint8_t count) -{ - // make sure that the requested offset matches our current position - off_t pos = lseek(_fd, 0, SEEK_CUR); - if (pos != offset) { - return -1; - } - return write(_fd, buf, count); + for (size_t i=0; iget_system_id(), _mavlink->get_component_id(), _mavlink->get_channel(), &msg, sequence(), rawData()); - if (!_mavlink->message_buffer_write(&msg, len)) { + _mavlink->lockMessageBufferMutex(); + bool fError = _mavlink->message_buffer_write(&msg, len); + _mavlink->unlockMessageBufferMutex(); + + if (!fError) { warnx("FTP TX ERR"); } else { warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", @@ -211,6 +199,8 @@ private: ErrorCode _workRead(Request *req); ErrorCode _workWrite(Request *req); ErrorCode _workRemove(Request *req); + ErrorCode _workTerminate(Request *req); + ErrorCode _workReset(); // work freelist Request _workBufs[kRequestQueueSize]; @@ -232,4 +222,5 @@ private: _qUnlock(); return req; } + }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e9cce175d..c77f3c6ca 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2074,38 +2074,46 @@ Mavlink::task_main(int argc, char *argv[]) if (_passing_on || _ftp_on) { bool is_part; - void *read_ptr; + uint8_t *read_ptr; + uint8_t *write_ptr; - /* guard get ptr by mutex */ pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr(&read_ptr, &is_part); + int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { - - // int oldseq = mavlink_get_channel_status(get_channel())->current_tx_seq; - - const mavlink_message_t* msg = (const mavlink_message_t*)read_ptr; - /* write first part of buffer */ - _mavlink_resend_uart(_channel, msg); - - // mavlink_get_channel_status(get_channel())->current_tx_seq = oldseq; - // mavlink_msg_system_time_send(get_channel(), 255, 255); - - message_buffer_mark_read(available); - + // Reconstruct message from buffer + + mavlink_message_t msg; + write_ptr = (uint8_t*)&msg; + + // Pull a single message from the buffer + int read_count = available; + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); + + message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ - // XXX this doesn't quite work, as the resend UART call assumes a continous block - if (is_part) { - /* guard get ptr by mutex */ - pthread_mutex_lock(&_message_buffer_mutex); - available = message_buffer_get_ptr(&read_ptr, &is_part); - pthread_mutex_unlock(&_message_buffer_mutex); - - _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + if (is_part && read_count < sizeof(mavlink_message_t)) { + write_ptr += read_count; + available = message_buffer_get_ptr((void**)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } + + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, &msg); } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 76ce42943..d44db0819 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -228,6 +228,9 @@ public: bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } bool message_buffer_write(void *ptr, int size); + + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } protected: Mavlink *next; -- cgit v1.2.3 From 196edd8a4faa36f837ff440ed41fc9656f96e20f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Jun 2014 12:12:08 +0200 Subject: estimator: Fix minor reporting issues --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index f1ae3f5e5..334177ad8 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -644,7 +644,7 @@ FixedwingEstimator::check_filter_state() rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); - rep.health_flags |= (((uint8_t)ekf_report.gyroOffsetsExcessive) << 3); + rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); @@ -655,14 +655,14 @@ FixedwingEstimator::check_filter_state() if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s", - ((rep.timeout_flags & (1 << 0)) ? "OK" : "ERR"), - ((rep.timeout_flags & (1 << 1)) ? "OK" : "ERR"), - ((rep.timeout_flags & (1 << 2)) ? "OK" : "ERR"), - ((rep.timeout_flags & (1 << 3)) ? "OK" : "ERR")); + ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); } if (rep.timeout_flags) { - warnx("timeout: %s%s%s", + warnx("timeout: %s%s%s%s", ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), -- cgit v1.2.3 From 503ded05394767d58359834e73bc63672b701dbe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Jun 2014 12:35:04 +0200 Subject: Remove old TECS implementation - we can really only decently flight-test and support one. --- makefiles/config_aerocore_default.mk | 1 - makefiles/config_px4fmu-v1_default.mk | 1 - makefiles/config_px4fmu-v2_default.mk | 1 - .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 72 +++++----------------- 4 files changed, 16 insertions(+), 59 deletions(-) diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index d6902c4ee..fd112b32d 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -80,7 +80,6 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl -MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion MODULES += lib/launchdetection diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index e4bc6fecf..f943f62f2 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -107,7 +107,6 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl -MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion MODULES += lib/launchdetection diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a68cc32f4..83dd85adb 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -120,7 +120,6 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl -MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion MODULES += lib/launchdetection diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5df17e2ec..f2c5db806 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -43,8 +43,8 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Original implementation for total energy control class: - * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) + * Implementation for total energy control class: + * Thomas Gubler * * More details and acknowledgements in the referenced library headers. * @@ -88,7 +88,6 @@ #include #include #include -#include #include #include "landingslope.h" #include "mtecs/mTecs.h" @@ -201,7 +200,6 @@ private: math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; - TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; @@ -568,23 +566,6 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); - _tecs.set_time_const(_parameters.time_const); - _tecs.set_min_sink_rate(_parameters.min_sink_rate); - _tecs.set_max_sink_rate(_parameters.max_sink_rate); - _tecs.set_throttle_damp(_parameters.throttle_damp); - _tecs.set_integrator_gain(_parameters.integrator_gain); - _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); - _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); - _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); - _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); - _tecs.set_speed_weight(_parameters.speed_weight); - _tecs.set_pitch_damping(_parameters.pitch_damping); - _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); - _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); - _tecs.set_max_climb_rate(_parameters.max_climb_rate); - _tecs.set_heightrate_p(_parameters.heightrate_p); - _tecs.set_speedrate_p(_parameters.speedrate_p); - /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -656,9 +637,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } - /* update TECS state */ - _tecs.enable_airspeed(_airspeed_valid); - return airspeed_updated; } @@ -839,10 +817,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - if (!_mTecs.getEnabled()) { - _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - } - float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ @@ -867,9 +841,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); - /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ - _tecs.set_speed_weight(_parameters.speed_weight); - /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); @@ -1222,8 +1193,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* user switched off throttle */ if (_manual.z < 0.1f) { throttle_max = 0.0f; - /* switch to pure pitch based altitude control, give up speed */ - _tecs.set_speed_weight(0.0f); } /* climb out control */ @@ -1263,9 +1232,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); } - _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + _att_sp.pitch_body = _mTecs.getPitchSetpoint(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1449,29 +1418,20 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - if (_mTecs.getEnabled()) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - } - fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); - } else { - limitOverride.disablePitchMinOverride(); - } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); } else { - /* Using tecs library */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, - _airspeed.indicated_airspeed_m_s, eas2tas, - climbout_mode, climbout_pitch_min_rad, - throttle_min, throttle_max, throttle_cruise, - pitch_min_rad, pitch_max_rad); + limitOverride.disablePitchMinOverride(); } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); } int -- cgit v1.2.3 From 2bd5511d096c3351bf74c81072e2cc00b344ef79 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Jun 2014 12:37:26 +0200 Subject: geo lookup lib: Moved to separate module and compiling with -Os to save some precious flash --- src/lib/geo/geo.h | 2 +- src/lib/geo/geo_mag_declination.c | 136 ------------------------------- src/lib/geo/geo_mag_declination.h | 47 ----------- src/lib/geo/module.mk | 5 +- src/lib/geo_lookup/geo_mag_declination.c | 136 +++++++++++++++++++++++++++++++ src/lib/geo_lookup/geo_mag_declination.h | 47 +++++++++++ src/lib/geo_lookup/module.mk | 40 +++++++++ 7 files changed, 226 insertions(+), 187 deletions(-) delete mode 100644 src/lib/geo/geo_mag_declination.c delete mode 100644 src/lib/geo/geo_mag_declination.h create mode 100644 src/lib/geo_lookup/geo_mag_declination.c create mode 100644 src/lib/geo_lookup/geo_mag_declination.h create mode 100644 src/lib/geo_lookup/module.mk diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 87c1cf460..8b286af36 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -50,7 +50,7 @@ __BEGIN_DECLS -#include "geo_mag_declination.h" +#include "geo_lookup/geo_mag_declination.h" #include diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c deleted file mode 100644 index 09eac38f4..000000000 --- a/src/lib/geo/geo_mag_declination.c +++ /dev/null @@ -1,136 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name MAVGEO nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** -* @file geo_mag_declination.c -* -* Calculation / lookup table for earth magnetic field declination. -* -* Lookup table from Scott Ferguson -* -* XXX Lookup table currently too coarse in resolution (only full degrees) -* and lat/lon res - needs extension medium term. -* -*/ - -#include - -/** set this always to the sampling in degrees for the table below */ -#define SAMPLING_RES 10.0f -#define SAMPLING_MIN_LAT -60.0f -#define SAMPLING_MAX_LAT 60.0f -#define SAMPLING_MIN_LON -180.0f -#define SAMPLING_MAX_LON 180.0f - -static const int8_t declination_table[13][37] = \ -{ - 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \ - -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \ - -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \ - 29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \ - -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \ - 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \ - 10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \ - -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \ - -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \ - 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \ - 7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \ - 0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \ - 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \ - 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \ - -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \ - 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \ - 0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \ - 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 -}; - -static float get_lookup_table_val(unsigned lat, unsigned lon); - -__EXPORT float get_mag_declination(float lat, float lon) -{ - /* - * If the values exceed valid ranges, return zero as default - * as we have no way of knowing what the closest real value - * would be. - */ - if (lat < -90.0f || lat > 90.0f || - lon < -180.0f || lon > 180.0f) { - return 0.0f; - } - - /* round down to nearest sampling resolution */ - int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES; - int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES; - - /* for the rare case of hitting the bounds exactly - * the rounding logic wouldn't fit, so enforce it. - */ - - /* limit to table bounds - required for maxima even when table spans full globe range */ - if (lat <= SAMPLING_MIN_LAT) { - min_lat = SAMPLING_MIN_LAT; - } - - if (lat >= SAMPLING_MAX_LAT) { - min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; - } - - if (lon <= SAMPLING_MIN_LON) { - min_lon = SAMPLING_MIN_LON; - } - - if (lon >= SAMPLING_MAX_LON) { - min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; - } - - /* find index of nearest low sampling point */ - unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES; - unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES; - - float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); - float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); - float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1); - float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index); - - /* perform bilinear interpolation on the four grid corners */ - - float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; - float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; - - return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min; -} - -float get_lookup_table_val(unsigned lat_index, unsigned lon_index) -{ - return declination_table[lat_index][lon_index]; -} \ No newline at end of file diff --git a/src/lib/geo/geo_mag_declination.h b/src/lib/geo/geo_mag_declination.h deleted file mode 100644 index 0ac062d6d..000000000 --- a/src/lib/geo/geo_mag_declination.h +++ /dev/null @@ -1,47 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name MAVGEO nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** -* @file geo_mag_declination.h -* -* Calculation / lookup table for earth magnetic field declination. -* -*/ - -#pragma once - -__BEGIN_DECLS - -__EXPORT float get_mag_declination(float lat, float lon); - -__END_DECLS diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk index 9500a2bcc..d08ca4532 100644 --- a/src/lib/geo/module.mk +++ b/src/lib/geo/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -35,5 +35,4 @@ # Geo library # -SRCS = geo.c \ - geo_mag_declination.c +SRCS = geo.c diff --git a/src/lib/geo_lookup/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c new file mode 100644 index 000000000..09eac38f4 --- /dev/null +++ b/src/lib/geo_lookup/geo_mag_declination.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name MAVGEO nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file geo_mag_declination.c +* +* Calculation / lookup table for earth magnetic field declination. +* +* Lookup table from Scott Ferguson +* +* XXX Lookup table currently too coarse in resolution (only full degrees) +* and lat/lon res - needs extension medium term. +* +*/ + +#include + +/** set this always to the sampling in degrees for the table below */ +#define SAMPLING_RES 10.0f +#define SAMPLING_MIN_LAT -60.0f +#define SAMPLING_MAX_LAT 60.0f +#define SAMPLING_MIN_LON -180.0f +#define SAMPLING_MAX_LON 180.0f + +static const int8_t declination_table[13][37] = \ +{ + 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \ + -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \ + -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \ + 29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \ + -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \ + 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \ + 10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \ + -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \ + -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \ + 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \ + 7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \ + 0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \ + 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \ + 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \ + -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \ + 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \ + 0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \ + 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 +}; + +static float get_lookup_table_val(unsigned lat, unsigned lon); + +__EXPORT float get_mag_declination(float lat, float lon) +{ + /* + * If the values exceed valid ranges, return zero as default + * as we have no way of knowing what the closest real value + * would be. + */ + if (lat < -90.0f || lat > 90.0f || + lon < -180.0f || lon > 180.0f) { + return 0.0f; + } + + /* round down to nearest sampling resolution */ + int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES; + int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES; + + /* for the rare case of hitting the bounds exactly + * the rounding logic wouldn't fit, so enforce it. + */ + + /* limit to table bounds - required for maxima even when table spans full globe range */ + if (lat <= SAMPLING_MIN_LAT) { + min_lat = SAMPLING_MIN_LAT; + } + + if (lat >= SAMPLING_MAX_LAT) { + min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + } + + if (lon <= SAMPLING_MIN_LON) { + min_lon = SAMPLING_MIN_LON; + } + + if (lon >= SAMPLING_MAX_LON) { + min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + } + + /* find index of nearest low sampling point */ + unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES; + unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES; + + float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); + float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); + float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1); + float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index); + + /* perform bilinear interpolation on the four grid corners */ + + float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; + float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; + + return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min; +} + +float get_lookup_table_val(unsigned lat_index, unsigned lon_index) +{ + return declination_table[lat_index][lon_index]; +} \ No newline at end of file diff --git a/src/lib/geo_lookup/geo_mag_declination.h b/src/lib/geo_lookup/geo_mag_declination.h new file mode 100644 index 000000000..0ac062d6d --- /dev/null +++ b/src/lib/geo_lookup/geo_mag_declination.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name MAVGEO nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file geo_mag_declination.h +* +* Calculation / lookup table for earth magnetic field declination. +* +*/ + +#pragma once + +__BEGIN_DECLS + +__EXPORT float get_mag_declination(float lat, float lon); + +__END_DECLS diff --git a/src/lib/geo_lookup/module.mk b/src/lib/geo_lookup/module.mk new file mode 100644 index 000000000..d7a10df2d --- /dev/null +++ b/src/lib/geo_lookup/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Geo lookup table / data library +# + +SRCS = geo_mag_declination.c + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 13f9ce9ddd0f6ab4b8fd4f7be3234e6989da8ea9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Jun 2014 12:38:31 +0200 Subject: Comment fix in perf counter header, no code changes. --- src/modules/systemlib/perf_counter.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 6835ee4a2..668d9dfdf 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -75,7 +75,7 @@ __EXPORT extern void perf_free(perf_counter_t handle); /** * Count a performance event. * - * This call only affects counters that take single events; PC_COUNT etc. + * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc. * * @param handle The handle returned from perf_alloc. */ -- cgit v1.2.3 From e78d496e921e9c50a3efc48a0a5875be1616c788 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Jun 2014 12:51:22 +0200 Subject: Enable new lookup lib --- makefiles/config_aerocore_default.mk | 1 + makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + 3 files changed, 3 insertions(+) diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index d6902c4ee..0d3934bd0 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -82,6 +82,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/geo_lookup MODULES += lib/conversion MODULES += lib/launchdetection diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 810e008ae..9c7f32632 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -113,6 +113,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/geo_lookup MODULES += lib/conversion MODULES += lib/launchdetection diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a68cc32f4..29fb9ebac 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -122,6 +122,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/geo_lookup MODULES += lib/conversion MODULES += lib/launchdetection -- cgit v1.2.3 From 0a78206d71f4c008fdad3a1170ab2bf7904548ed Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 28 Jun 2014 18:45:52 +0200 Subject: att pos estimator: silence annoying messages --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index cb160c775..987b4f1c3 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -559,7 +559,7 @@ FixedwingEstimator::check_filter_state() int check = _ekf->CheckAndBound(&ekf_report); - const char* ekfname = "#audio: "; + const char* ekfname = "att pos estimator: "; switch (check) { case 0: -- cgit v1.2.3 From 336fc2fcf5020f296ec760ca44c2c706b57c61ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 11:27:09 +0200 Subject: Fix compile warnings in estimator --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 987b4f1c3..e4f805dc0 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -49,11 +49,11 @@ #include #include #include -#include +#include #define SENSOR_COMBINED_SUB - +#include #include #include #include @@ -146,7 +146,7 @@ public: * * @param debug Desired debug level - 0 to disable. */ - int set_debuglevel(unsigned debug) { _debug = debug; } + int set_debuglevel(unsigned debug) { _debug = debug; return 0; } private: @@ -1256,9 +1256,9 @@ FixedwingEstimator::task_main() float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f; // Calculate acceleration predicted by GPS velocity change - if ((_ekf->velNED[0] != _gps.vel_n_m_s || - _ekf->velNED[1] != _gps.vel_e_m_s || - _ekf->velNED[2] != _gps.vel_d_m_s) && (gps_dt > 0.00001f)) { + if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || + (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || + (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; -- cgit v1.2.3 From 91bba668f670db7cec916966d3c53d3b25ac7061 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 11:27:43 +0200 Subject: Define earth radius as double, as our calculations relying on it need double precision. --- src/modules/ekf_att_pos_estimator/estimator_utilities.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 97f22b453..d47568b62 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -8,8 +8,8 @@ #define rad2deg 57.295780f #define pi 3.141592657f #define earthRate 0.000072921f -#define earthRadius 6378145.0f -#define earthRadiusInv 1.5678540e-7f +#define earthRadius 6378145.0 +#define earthRadiusInv 1.5678540e-7 class Vector3f { -- cgit v1.2.3 From cd2ef000ebba7da47a30fbdeb09635bef3a2684a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 11:35:27 +0200 Subject: More safe compile warning fixes --- .../ekf_att_pos_estimator/estimator_23states.cpp | 24 +++++++++++----------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 4c503b5d4..826419cda 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1418,7 +1418,7 @@ void AttPosEKF::FuseMagnetometer() } // normalise the quaternion states float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12) + if (quatMag > 1e-12f) { for (uint8_t j= 0; j<=3; j++) { @@ -1562,7 +1562,7 @@ void AttPosEKF::FuseAirspeed() // Calculate the measurement innovation innovVtas = VtasPred - VtasMeas; // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovVtas*innovVtas*SK_TAS) < 25.0) + if ((innovVtas*innovVtas*SK_TAS) < 25.0f) { // correct the state vector for (uint8_t j=0; j <= 22; j++) @@ -1827,7 +1827,7 @@ void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) Tnb.y.z = 2*(q23 + q01); } -void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) +void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4]) { // Calculate the body to nav cosine matrix float q00 = sq(quat[0]); @@ -1841,15 +1841,15 @@ void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) float q13 = quat[1]*quat[3]; float q23 = quat[2]*quat[3]; - Tbn.x.x = q00 + q11 - q22 - q33; - Tbn.y.y = q00 - q11 + q22 - q33; - Tbn.z.z = q00 - q11 - q22 + q33; - Tbn.x.y = 2*(q12 - q03); - Tbn.x.z = 2*(q13 + q02); - Tbn.y.x = 2*(q12 + q03); - Tbn.y.z = 2*(q23 - q01); - Tbn.z.x = 2*(q13 - q02); - Tbn.z.y = 2*(q23 + q01); + Tbn_ret.x.x = q00 + q11 - q22 - q33; + Tbn_ret.y.y = q00 - q11 + q22 - q33; + Tbn_ret.z.z = q00 - q11 - q22 + q33; + Tbn_ret.x.y = 2*(q12 - q03); + Tbn_ret.x.z = 2*(q13 + q02); + Tbn_ret.y.x = 2*(q12 + q03); + Tbn_ret.y.z = 2*(q23 - q01); + Tbn_ret.z.x = 2*(q13 - q02); + Tbn_ret.z.y = 2*(q23 + q01); } void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3]) -- cgit v1.2.3 From 092ede366a531ad68f7ccc2f372f83b8d2993242 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 11:55:44 +0200 Subject: Estimator: Clean up delta quat calculations, put them in a sweet spot between accuracy and runtime. --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 826419cda..df319a93a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -71,7 +71,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float rotationMag; float qUpdated[4]; float quatMag; - double deltaQuat[4]; + float deltaQuat[4]; const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; // Remove sensor bias errors @@ -104,8 +104,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED() } else { - deltaQuat[0] = cos(0.5f*rotationMag); - double rotScaler = (sin(0.5f*rotationMag))/rotationMag; + // We are using double here as we are unsure how small + // the angle differences are and if we get into numeric + // issues with float. The runtime impact is not measurable + // for these quantities. + deltaQuat[0] = cos(0.5*(double)rotationMag); + float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag; deltaQuat[1] = correctedDelAng.x*rotScaler; deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler; @@ -1659,7 +1663,7 @@ void AttPosEKF::FuseRangeFinder() // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data SH_RNG[4] = sin(rngFinderPitch); - cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch); + cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch); if (useRangeFinder && cosRngTilt > 0.87f) { // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset -- cgit v1.2.3 From b64c64d5a3d71a085b603e6f86dfe785a03c6129 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 12:00:54 +0200 Subject: Observation index cannot get negative. --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index df319a93a..8c30521e3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1088,7 +1088,7 @@ void AttPosEKF::FuseVelposNED() stateIndex = 4 + obsIndex; // Calculate the measurement innovation, using states from a // different time coordinate if fusing height data - if (obsIndex >= 0 && obsIndex <= 2) + if (obsIndex <= 2) { innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; } -- cgit v1.2.3 From ccdfb19a4ac7c83643a013e3acf11ff745bd975f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 12:05:27 +0200 Subject: estimator: Fix double promotion warnings by appropriate constants / casts. --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 8c30521e3..7a34c82f9 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1103,7 +1103,7 @@ void AttPosEKF::FuseVelposNED() // Calculate the Kalman Gain // Calculate innovation variances - also used for data logging varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; - SK = 1.0/varInnovVelPos[obsIndex]; + SK = 1.0/(double)varInnovVelPos[obsIndex]; for (uint8_t i= 0; i<=indexLimit; i++) { Kfusion[i] = P[i][stateIndex]*SK; @@ -1413,7 +1413,7 @@ void AttPosEKF::FuseMagnetometer() } // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) + if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f) { // correct the state vector for (uint8_t j= 0; j < indexLimit; j++) @@ -2130,7 +2130,7 @@ bool AttPosEKF::GyroOffsetsDiverged() // Protect against division by zero if (delta_len > 0.0f) { float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f); - delta_len_scaled = (5e-7 / cov_mag) * delta_len / dtIMU; + delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU; } bool diverged = (delta_len_scaled > 1.0f); -- cgit v1.2.3 From d93a64dd09a34515e4d9812408a530f3c9721a46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 12:08:28 +0200 Subject: estimator: Geo conversions: Fix float / double types to prevent warnings. Fix calcLLH to actually return a value. --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 14 +++++++------- src/modules/ekf_att_pos_estimator/estimator_23states.h | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 7a34c82f9..0894d60ae 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1884,17 +1884,17 @@ void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, velNED[2] = gpsVelD; } -void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef) +void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference) { - posNED[0] = earthRadius * (lat - latRef); - posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); - posNED[2] = -(hgt - hgtRef); + posNED[0] = earthRadius * (lat - latReference); + posNED[1] = earthRadius * cos(latReference) * (lon - lonReference); + posNED[2] = -(hgt - hgtReference); } -void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void AttPosEKF::calcLLH(float (&posNED)[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef) { - lat = latRef + posNED[0] * earthRadiusInv; - lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); + lat = latRef + (double)posNED[0] * earthRadiusInv; + lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef); hgt = hgtRef - posNED[2]; } diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 5a1f5125a..dc461cfa1 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -246,7 +246,7 @@ static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, flo void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); -static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcLLH(float (&posNED)[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); -- cgit v1.2.3 From 2b3241470996d333d78fa9e78fea9b35fee7e18c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 12:13:40 +0200 Subject: estimator: Move symmetry force outside of non-related loop, improving efficiency and resolving a locals warning. --- .../ekf_att_pos_estimator/estimator_23states.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 0894d60ae..7f5968363 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -887,20 +887,20 @@ void AttPosEKF::CovariancePrediction(float dt) // propagate for (unsigned i = 0; i <= 13; i++) { P[i][i] = nextP[i][i]; + } - // force symmetry for observable states - // force zero for non-observable states - for (unsigned i = 1; i < n_states; i++) + // force symmetry for observable states + // force zero for non-observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) { - for (uint8_t j = 0; j < i; j++) - { - if ((i > 13) || (j > 13)) { - P[i][j] = 0.0f; - } else { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - } - P[j][i] = P[i][j]; + if ((i > 13) || (j > 13)) { + P[i][j] = 0.0f; + } else { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); } + P[j][i] = P[i][j]; } } -- cgit v1.2.3 From 8d1ed164cb6efc533da9ef8ba6c94a00ef329d30 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Jun 2014 12:51:40 +0200 Subject: fw att ctrl: resolve warnings --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 3 --- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 3 --- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 7 +------ 3 files changed, 1 insertion(+), 12 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index a0a18bc2e..46db788a6 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -151,9 +151,6 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (dt_micros > 500000) lock_integrator = true; -// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_ff = 0; - /* input conditioning */ if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index d2a231694..9894a34d7 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -114,9 +114,6 @@ float ECL_RollController::control_bodyrate(float pitch, if (dt_micros > 500000) lock_integrator = true; -// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_ff = 0; //xxx: param - /* input conditioning */ // warnx("airspeed pre %.4f", (double)airspeed); if (!isfinite(airspeed)) { diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 79184e2cd..fe03b8065 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -84,7 +84,7 @@ float ECL_YawController::control_attitude(float roll, float pitch, _rate_setpoint = 0.0f; if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) { float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); - if(denumerator != 0.0f) { //XXX: floating point comparison + if(fabsf(denumerator) > FLT_EPSILON) { _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; // warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint); } @@ -132,11 +132,6 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, if (dt_micros > 500000) lock_integrator = true; - -// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_ff = 0; - - /* input conditioning */ if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ -- cgit v1.2.3 From 0426fd3a52888277bcb612c57488e9de33432134 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 12:59:04 +0200 Subject: estimator: Fix local variable compile warnings. --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 7f5968363..e4b0c2b14 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1760,21 +1760,21 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) int64_t bestTimeDelta = 200; unsigned bestStoreIndex = 0; - for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++) { // Work around a GCC compiler bug - we know 64bit support on ARM is // sketchy in GCC. uint64_t timeDelta; - if (msec > statetimeStamp[storeIndex]) { - timeDelta = msec - statetimeStamp[storeIndex]; + if (msec > statetimeStamp[storeIndexLocal]) { + timeDelta = msec - statetimeStamp[storeIndexLocal]; } else { - timeDelta = statetimeStamp[storeIndex] - msec; + timeDelta = statetimeStamp[storeIndexLocal] - msec; } - if (timeDelta < bestTimeDelta) + if (timeDelta < (uint64_t)bestTimeDelta) { - bestStoreIndex = storeIndex; + bestStoreIndex = storeIndexLocal; bestTimeDelta = timeDelta; } } -- cgit v1.2.3 From 12be974bd67676ef243d069593b179108976da22 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Jun 2014 13:07:38 +0200 Subject: navigator: whitespace fix --- src/modules/navigator/mission.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 395669698..ab99a6b7e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -255,7 +255,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: no mission available"); } _mission_type = MISSION_TYPE_NONE; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); set_loiter_item(pos_sp_triplet); pos_sp_triplet->previous.valid = false; -- cgit v1.2.3 From 628477ee2c75d16758688f53c0f8d9f29457f831 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 14:01:58 +0200 Subject: Update estimator from Paul --- .../ekf_att_pos_estimator/estimator_23states.cpp | 610 ++++++++++++++++----- .../ekf_att_pos_estimator/estimator_23states.h | 20 +- .../ekf_att_pos_estimator/estimator_utilities.cpp | 19 + .../ekf_att_pos_estimator/estimator_utilities.h | 3 +- 4 files changed, 509 insertions(+), 143 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index e4b0c2b14..d77f4a6e3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -25,6 +25,7 @@ AttPosEKF::AttPosEKF() useAirspeed = true; useCompass = true; useRangeFinder = true; + useOpticalFlow = true; numericalProtection = true; refSet = false; storeIndex = 0; @@ -227,10 +228,22 @@ void AttPosEKF::CovariancePrediction(float dt) // scale gyro bias noise when on ground to allow for faster bias estimation for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; processNoise[13] = dVelBiasSigma; - for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; - for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; - for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; - processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + if (!inhibitWindStates) { + for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; + } else { + for (uint8_t i=14; i<=15; i++) processNoise[i] = 0; + } + if (!inhibitMagStates) { + for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; + for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; + } else { + for (uint8_t i=16; i<=21; i++) processNoise[i] = 0; + } + if (!inhibitGndHgtState) { + processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + } else { + processNoise[22] = 0; + } // square all sigmas for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); @@ -842,30 +855,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[i][i] = nextP[i][i] + processNoise[i]; } - // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by - // setting the coresponding covariance terms to zero. - if (onGround || !useCompass) - { - zeroRows(nextP,16,21); - zeroCols(nextP,16,21); - } - - // If on ground or not using airspeed sensing, inhibit wind velocity - // covariance growth. - if (onGround || !useAirspeed) - { - zeroRows(nextP,14,15); - zeroCols(nextP,14,15); - } - - // If on ground, inhibit terrain offset updates by - // setting the coresponding covariance terms to zero. - if (onGround) - { - zeroRows(nextP,22,22); - zeroCols(nextP,22,22); - } - // If the total position variance exceds 1E6 (1000m), then stop covariance // growth by setting the predicted to the previous values // This prevent an ill conditioned matrix from occurring for long periods @@ -882,48 +871,22 @@ void AttPosEKF::CovariancePrediction(float dt) } } - if (onGround || staticMode) { - // copy the portion of the variances we want to - // propagate - for (unsigned i = 0; i <= 13; i++) { - P[i][i] = nextP[i][i]; - } - - // force symmetry for observable states - // force zero for non-observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - if ((i > 13) || (j > 13)) { - P[i][j] = 0.0f; - } else { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - } - P[j][i] = P[i][j]; - } - } - - } else { - - // Copy covariance - for (unsigned i = 0; i < n_states; i++) { - P[i][i] = nextP[i][i]; - } + // Copy covariance + for (unsigned i = 0; i < n_states; i++) { + P[i][i] = nextP[i][i]; + } - // force symmetry for observable states - for (unsigned i = 1; i < n_states; i++) + // force symmetry for observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - P[j][i] = P[i][j]; - } + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; } - } - ConstrainVariances(); + ConstrainVariances(); } void AttPosEKF::FuseVelposNED() @@ -944,7 +907,7 @@ void AttPosEKF::FuseVelposNED() bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; uint8_t obsIndex; - uint8_t indexLimit; + uint8_t indexLimit = 22; // declare variables used by state and covariance update calculations float velErr; @@ -981,11 +944,6 @@ void AttPosEKF::FuseVelposNED() R_OBS[4] = R_OBS[3]; R_OBS[5] = sq(posDSigma) + sq(posErr); - // Set innovation variances to zero default - for (uint8_t i = 0; i<=5; i++) - { - varInnovVelPos[i] = 0.0f; - } // calculate innovations and check GPS data validity using an innovation consistency check if (fuseVelData) { @@ -1071,15 +1029,6 @@ void AttPosEKF::FuseVelposNED() { fuseData[5] = true; } - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = 22; - } - else - { - indexLimit = 13; - } // Fuse measurements sequentially for (obsIndex=0; obsIndex<=5; obsIndex++) { @@ -1113,6 +1062,22 @@ void AttPosEKF::FuseVelposNED() if (obsIndex != 5) { Kfusion[13] = 0; } + // Don't update wind states if inhibited + if (inhibitWindStates) { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + // Don't update magnetic field states if inhibited + if (inhibitMagStates) { + for (uint8_t i = 16; i<=21; i++) + { + Kfusion[i] = 0; + } + } + // Don't update terrain state if inhibited + if (inhibitGndHgtState) { + Kfusion[22] = 0; + } // Calculate state corrections and re-normalise the quaternions for (uint8_t i = 0; i<=indexLimit; i++) @@ -1179,7 +1144,6 @@ void AttPosEKF::FuseMagnetometer() for (uint8_t i = 0; i < n_states; i++) { H_MAG[i] = 0.0f; } - unsigned indexLimit; // Perform sequential fusion of Magnetometer measurements. // This assumes that the errors in the different components are @@ -1189,19 +1153,6 @@ void AttPosEKF::FuseMagnetometer() // associated with sequential fusion if (useCompass && fuseMagData && (obsIndex < 3)) { - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = n_states; - } - else - { - indexLimit = 13 + 1; - } - - // Sequential fusion of XYZ components to spread processing load across - // three prediction time steps. - // Calculate observation jacobians and Kalman gains if (obsIndex == 0) { @@ -1287,15 +1238,31 @@ void AttPosEKF::FuseMagnetometer() Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]); // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]); - Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); - Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); - Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); - Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); - Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); - Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); - Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); - Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); - Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); + // Estimation of selected states is inhibited by setting their Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); + } else { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + if (!inhibitMagStates) { + Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); + Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); + } else { + for (uint8_t i=16; i <= 21; i++) { + Kfusion[i] = 0; + } + } + if (!inhibitGndHgtState) { + Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); + } else { + Kfusion[22] = 0; + } varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; } @@ -1342,15 +1309,34 @@ void AttPosEKF::FuseMagnetometer() Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); - Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); - Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); - Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); - Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); - Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); - Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); - Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); - Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); - Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); + // Estimation of selected states is inhibited by setting their Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); + Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); + } else { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + if (!inhibitMagStates) { + Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); + Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); + Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); + Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); + Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); + Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); + } else { + Kfusion[16] = 0; + Kfusion[17] = 0; + Kfusion[18] = 0; + Kfusion[19] = 0; + Kfusion[20] = 0; + Kfusion[21] = 0; + } + if (!inhibitGndHgtState) { + Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); + } else { + Kfusion[22] = 0; + } varInnovMag[1] = 1.0f/SK_MY[0]; innovMag[1] = MagPred[1] - magData.y; } @@ -1398,15 +1384,34 @@ void AttPosEKF::FuseMagnetometer() Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]); // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]); - Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]); - Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]); - Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]); - Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]); - Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]); - Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]); - Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]); - Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]); - Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]); + // Estimation of selected states is inhibited by setting their Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]); + Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]); + } else { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + if (!inhibitMagStates) { + Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]); + Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]); + Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]); + Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]); + Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]); + Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]); + } else { + Kfusion[16] = 0; + Kfusion[17] = 0; + Kfusion[18] = 0; + Kfusion[19] = 0; + Kfusion[20] = 0; + Kfusion[21] = 0; + } + if (!inhibitGndHgtState) { + Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]); + } else { + Kfusion[22] = 0; + } varInnovMag[2] = 1.0f/SK_MZ[0]; innovMag[2] = MagPred[2] - magData.z; @@ -1416,7 +1421,7 @@ void AttPosEKF::FuseMagnetometer() if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f) { // correct the state vector - for (uint8_t j= 0; j < indexLimit; j++) + for (uint8_t j= 0; j < n_states; j++) { states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; } @@ -1433,7 +1438,7 @@ void AttPosEKF::FuseMagnetometer() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations - for (uint8_t i = 0; i < indexLimit; i++) + for (uint8_t i = 0; i < n_states; i++) { for (uint8_t j = 0; j <= 3; j++) { @@ -1455,9 +1460,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i < indexLimit; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j < indexLimit; j++) + for (uint8_t j = 0; j < n_states; j++) { KHP[i][j] = 0.0f; for (uint8_t k = 0; k <= 3; k++) @@ -1474,9 +1479,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i < indexLimit; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j < indexLimit; j++) + for (uint8_t j = 0; j < n_states; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1552,15 +1557,31 @@ void AttPosEKF::FuseAirspeed() Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); - Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); - Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); - Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); - Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); - Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); - Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); - Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); - Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); - Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]); + // Estimation of selected states is inhibited by setting their Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); + } else { + Kfusion[14] = 0; + Kfusion[15] = 0; + } + if (!inhibitMagStates) { + Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); + Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); + } else { + for (uint8_t i=16; i <= 21; i++) { + Kfusion[i] = 0; + } + } + if (!inhibitGndHgtState) { + Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]); + } else { + Kfusion[22] = 0; + } varInnovVtas = 1.0f/SK_TAS; // Calculate the measurement innovation @@ -1662,9 +1683,9 @@ void AttPosEKF::FuseRangeFinder() float ptd = statesAtRngTime[22]; // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data - SH_RNG[4] = sin(rngFinderPitch); + SH_RNG[4] = sinf(rngFinderPitch); cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch); - if (useRangeFinder && cosRngTilt > 0.87f) + if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) { // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations @@ -1685,10 +1706,12 @@ void AttPosEKF::FuseRangeFinder() SK_RNG[5] = SH_RNG[2]; Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); + // Calculate the innovation variance for data logging + varInnovRng = 1.0f/SK_RNG[0]; + // Calculate the measurement innovation rngPred = (ptd - pd)/cosRngTilt; innovRng = rngPred - rngMea; - //printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd); // Check the innovation for consistency and don't fuse if > 5Sigma if ((innovRng*innovRng*SK_RNG[0]) < 25) @@ -1704,6 +1727,293 @@ void AttPosEKF::FuseRangeFinder() } +void AttPosEKF::FuseOptFlow() +{ + static uint8_t obsIndex; + static float SH_LOS[13]; + static float SKK_LOS[15]; + static float SK_LOS[2]; + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float vn = 0.0f; + static float ve = 0.0f; + static float vd = 0.0f; + static float pd = 0.0f; + static float ptd = 0.0f; + static Vector3f delAng; + static float R_LOS = 0.01f; + static float losPred[2]; + + // Transformation matrix from nav to body axes + Mat3f Tnb; + // Transformation matrix from body to sensor axes + // assume camera is aligned with Z body axis plus a misalignment + // defined by 3 small angles about X, Y and Z body axis + Mat3f Tbs; + Tbs.x.y = a3; + Tbs.y.x = -a3; + Tbs.x.z = -a2; + Tbs.z.x = a2; + Tbs.y.z = a1; + Tbs.z.y = -a1; + // Transformation matrix from navigation to sensor axes + Mat3f Tns; + float H_LOS[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_LOS[i] = 0.0f; + } + Vector3f velNED; + Vector3f relVelSensor; + +// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. + if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f) + { + // Sequential fusion of XY components to spread processing load across + // two prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseOptFlowData) + { + // Copy required states to local variable names + q0 = statesAtOptFlowTime[0]; + q1 = statesAtOptFlowTime[1]; + q2 = statesAtOptFlowTime[2]; + q3 = statesAtOptFlowTime[3]; + vn = statesAtOptFlowTime[4]; + ve = statesAtOptFlowTime[5]; + vd = statesAtOptFlowTime[6]; + pd = statesAtOptFlowTime[9]; + ptd = statesAtOptFlowTime[22]; + velNED.x = vn; + velNED.y = ve; + velNED.z = vd; + + // calculate rotation from NED to body axes + float q00 = sq(q0); + float q11 = sq(q1); + float q22 = sq(q2); + float q33 = sq(q3); + float q01 = q0 * q1; + float q02 = q0 * q2; + float q03 = q0 * q3; + float q12 = q1 * q2; + float q13 = q1 * q3; + float q23 = q2 * q3; + Tnb.x.x = q00 + q11 - q22 - q33; + Tnb.y.y = q00 - q11 + q22 - q33; + Tnb.z.z = q00 - q11 - q22 + q33; + Tnb.y.x = 2*(q12 - q03); + Tnb.z.x = 2*(q13 + q02); + Tnb.x.y = 2*(q12 + q03); + Tnb.z.y = 2*(q23 - q01); + Tnb.x.z = 2*(q13 - q02); + Tnb.y.z = 2*(q23 + q01); + + // calculate transformation from NED to sensor axes + Tns = Tbs*Tnb; + + // calculate range from ground plain to centre of sensor fov assuming flat earth + float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f); + + // calculate relative velocity in sensor frame + relVelSensor = Tns*velNED; + + // calculate delta angles in sensor axes + Vector3f delAngRel = Tbs*delAng; + + // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes + losPred[0] = relVelSensor.y/range; + losPred[1] = -relVelSensor.x/range; + + //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y); + //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y); + //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range); + + // Calculate observation jacobians + SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); + SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + SH_LOS[3] = 1/(pd - ptd); + SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; + SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; + SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; + SH_LOS[7] = 1/sq(pd - ptd); + SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; + SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; + SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; + SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; + SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; + + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); + H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); + H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + + // Calculate Kalman gain + SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); + SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); + SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); + SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); + SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); + SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); + SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); + SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); + SKK_LOS[14] = SH_LOS[0]; + + SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14])); + Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); + varInnovOptFlow[0] = 1.0f/SK_LOS[0]; + innovOptFlow[0] = losPred[0] - losData[0]; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + fuseOptFlowData = false; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); + H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); + H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + + // Calculate Kalman gains + SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14])); + Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); + varInnovOptFlow[1] = 1.0f/SK_LOS[1]; + innovOptFlow[1] = losPred[1] - losData[1]; + } + + // Check the innovation for consistency and don't fuse if > 3Sigma + if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f) + { + // correct the state vector + for (uint8_t j = 0; j < n_states; j++) + { + states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j <= 6; j++) + { + KH[i][j] = Kfusion[i] * H_LOS[j]; + } + for (uint8_t j = 7; j <= 8; j++) + { + KH[i][j] = 0.0f; + } + KH[i][9] = Kfusion[i] * H_LOS[9]; + for (uint8_t j = 10; j <= 21; j++) + { + KH[i][j] = 0.0f; + } + KH[i][22] = Kfusion[i] * H_LOS[22]; + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; + KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; + } + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; + ForceSymmetry(); + ConstrainVariances(); +} + void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; @@ -1904,6 +2214,24 @@ void AttPosEKF::OnGroundCheck() if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } + // don't update wind states if there is no airspeed measurement + if (onGround || !useAirspeed) { + inhibitWindStates = true; + } else { + inhibitWindStates =false; + } + // don't update magnetic field states if on ground or not using compass + if (onGround || !useCompass) { + inhibitMagStates = true; + } else { + inhibitMagStates = false; + } + // don't update terrain offset state if on ground + if (onGround) { + inhibitGndHgtState = true; + } else { + inhibitGndHgtState = false; + } } void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) @@ -1931,8 +2259,8 @@ void AttPosEKF::CovarianceInit() P[11][11] = P[10][10]; P[12][12] = P[10][10]; P[13][13] = sq(0.2f*dtIMU); - P[14][14] = sq(8.0f); - P[15][14] = P[14][14]; + P[14][14] = sq(0.0f); + P[15][15] = P[14][14]; P[16][16] = sq(0.02f); P[17][17] = P[16][16]; P[18][18] = P[16][16]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index dc461cfa1..3110266ce 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -29,6 +29,10 @@ public: float covDelAngMax; // maximum delta angle between covariance predictions float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + float a1; // optical flow sensor misalgnment angle about X axis (rad) + float a2; // optical flow sensor misalgnment angle about Y axis (rad) + float a3; // optical flow sensor misalgnment angle about Z axis (rad) + float yawVarScale; float windVelSigma; float dAngBiasSigma; @@ -55,6 +59,9 @@ public: covDelAngMax = 0.02f; // maximum delta angle between covariance predictions rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. EAS2TAS = 1.0f; + a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad) + a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad) + a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad) yawVarScale = 1.0f; windVelSigma = 0.1f; @@ -115,6 +122,7 @@ public: float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float statesAtRngTime[n_states]; // filter states at the effective measurement time + float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) @@ -147,9 +155,13 @@ public: float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output Vector3f magData; // magnetometer flux radings in X,Y,Z body axes + float losData[2]; // optical flow LOS rate measurements (rad/sec) float innovVtas; // innovation output float innovRng; ///< Range finder innovation + float innovOptFlow[2]; // optical flow LOS innovations (rad/sec) + float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2 float varInnovVtas; // innovation variance output + float varInnovRng; // range finder innovation variance float VtasMeas; // true airspeed measurement (m/s) float magDeclination; ///< magnetic declination double latRef; // WGS-84 latitude of reference point (rad) @@ -178,12 +190,18 @@ public: bool fuseMagData; // boolean true when magnetometer data is to be fused bool fuseVtasData; // boolean true when airspeed data is to be fused bool fuseRngData; ///< true when range data is fused + bool fuseOptFlowData; // true when optical flow data is fused + + bool inhibitWindStates; // true when wind states and covariances are to remain constant + bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant + bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused bool useAirspeed; ///< boolean true if airspeed data is being used bool useCompass; ///< boolean true if magnetometer data is being used bool useRangeFinder; ///< true when rangefinder is being used + bool useOpticalFlow; // true when optical flow data is being used bool ekfDiverged; uint64_t lastReset; @@ -208,7 +226,7 @@ void FuseAirspeed(); void FuseRangeFinder(); -void FuseOpticalFlow(); +void FuseOptFlow(); void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index b4767a0d3..24168b84c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -101,6 +101,25 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) return vecOut; } +// overload * operator to provide a matrix product +Mat3f operator*( Mat3f matIn1, Mat3f matIn2) +{ + Mat3f matOut; + matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; + matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y; + matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z; + + matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x; + matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y; + matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z; + + matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x; + matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y; + matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z; + + return matOut; +} + // overload % operator to provide a vector cross product Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index d47568b62..5648cb05c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -41,6 +41,7 @@ Vector3f operator*(float sclIn1, Vector3f vecIn1); Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator*( Mat3f matIn, Vector3f vecIn); +Mat3f operator*( Mat3f matIn1, Mat3f matIn2); Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator*(Vector3f vecIn1, float sclIn1); @@ -79,4 +80,4 @@ struct ekf_status_report { bool velOffsetExcessive; }; -void ekf_debug(const char *fmt, ...); \ No newline at end of file +void ekf_debug(const char *fmt, ...); -- cgit v1.2.3 From 0bf9c2a9b262a4c8569031f0f7e9ded432d2d4b3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Jun 2014 14:09:22 +0200 Subject: navigator: API changes, reparing to move manual modes to navigator, WIP --- src/modules/navigator/loiter.cpp | 21 ++++++++----- src/modules/navigator/loiter.h | 4 +-- src/modules/navigator/mission.cpp | 42 ++++++++++++------------- src/modules/navigator/mission.h | 6 ++-- src/modules/navigator/mission_block.cpp | 53 +++++++++++++++----------------- src/modules/navigator/mission_block.h | 4 +-- src/modules/navigator/navigator.h | 4 ++- src/modules/navigator/navigator_main.cpp | 11 ++++--- src/modules/navigator/navigator_mode.cpp | 19 ++++++------ src/modules/navigator/navigator_mode.h | 9 ++---- 10 files changed, 87 insertions(+), 86 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 5e7067b0e..f827e70c9 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -36,6 +36,7 @@ * Helper class to loiter * * @author Julian Oes + * @author Anton Babushkin */ #include @@ -51,14 +52,13 @@ #include #include "loiter.h" +#include "navigator.h" Loiter::Loiter(Navigator *navigator, const char *name) : MissionBlock(navigator, name) { /* load initial params */ updateParams(); - /* initial reset */ - on_inactive(); } Loiter::~Loiter() @@ -71,16 +71,23 @@ Loiter::on_inactive() } void -Loiter::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +Loiter::on_activation() { - set_loiter_item(pos_sp_triplet); + /* set current mission item to loiter */ + set_loiter_item(&_mission_item); + + /* convert mission item to current setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + + _navigator->set_position_setpoint_triplet_updated(); } -bool -Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Loiter::on_active() { - return false; } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 5ce86d2a7..37ab57a07 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -56,9 +56,9 @@ public: virtual void on_inactive(); - virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); }; #endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ab99a6b7e..1e86b1b6c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -54,8 +54,8 @@ #include #include -#include "navigator.h" #include "mission.h" +#include "navigator.h" Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), @@ -97,16 +97,14 @@ Mission::on_inactive() } void -Mission::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +Mission::on_activation() { - set_mission_items(pos_sp_triplet); + set_mission_items(); } -bool -Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Mission::on_active() { - bool updated = false; - /* check if anything has changed */ bool onboard_updated; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); @@ -122,18 +120,18 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) /* reset mission items if needed */ if (onboard_updated || offboard_updated) { - set_mission_items(pos_sp_triplet); - updated = true; + set_mission_items(); + + _navigator->set_position_setpoint_triplet_updated(); } /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { advance_mission(); - set_mission_items(pos_sp_triplet); - updated = true; - } + set_mission_items(); - return updated; + _navigator->set_position_setpoint_triplet_updated(); + } } void @@ -223,8 +221,10 @@ Mission::advance_mission() } void -Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) +Mission::set_mission_items() { + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + set_previous_pos_setpoint(pos_sp_triplet); /* try setting onboard mission item */ @@ -235,7 +235,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: onboard mission running"); } _mission_type = MISSION_TYPE_ONBOARD; - _navigator->set_can_loiter_at_sp(false); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); /* try setting offboard mission item */ } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { @@ -245,7 +245,8 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: offboard mission running"); } _mission_type = MISSION_TYPE_OFFBOARD; - _navigator->set_can_loiter_at_sp(false); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); + } else { if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_info(_navigator->get_mavlink_fd(), @@ -255,12 +256,15 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: no mission available"); } _mission_type = MISSION_TYPE_NONE; + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); - set_loiter_item(pos_sp_triplet); + set_loiter_item(&_mission_item); + pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + reset_mission_item_reached(); report_mission_finished(); } @@ -279,7 +283,6 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current &new_mission_item)) { /* convert the current mission item and set it valid */ mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); - current_pos_sp->valid = true; reset_mission_item_reached(); @@ -306,7 +309,6 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) { /* convert the current mission item and set it valid */ mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); - current_pos_sp->valid = true; reset_mission_item_reached(); @@ -330,7 +332,6 @@ Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { /* convert next mission item to position setpoint */ mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - next_pos_sp->valid = true; return; } } @@ -358,7 +359,6 @@ Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) { /* convert next mission item to position setpoint */ mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - next_pos_sp->valid = true; return; } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 38a4f7612..ac293990f 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -71,9 +71,9 @@ public: virtual void on_inactive(); - virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); private: /** @@ -94,7 +94,7 @@ private: /** * Set new mission items */ - void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet); + void set_mission_items(); /** * Try to set the current position setpoint from an onboard mission item diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index acf3ad569..bf3c934a6 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -58,8 +58,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _mission_item({0}), - _mission_item_valid(false) + _mission_item({0}) { } @@ -70,6 +69,10 @@ MissionBlock::~MissionBlock() bool MissionBlock::is_mission_item_reached() { + if (_mission_item.nav_cmd == NAV_CMD_IDLE) { + return false; + } + if (_mission_item.nav_cmd == NAV_CMD_LAND) { return _navigator->get_vstatus()->condition_landed; } @@ -83,7 +86,6 @@ MissionBlock::is_mission_item_reached() hrt_abstime now = hrt_absolute_time(); if (!_waypoint_position_reached) { - float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; @@ -209,43 +211,38 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_ } void -MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) +MissionBlock::set_loiter_item(struct mission_item_s *item) { if (_navigator->get_vstatus()->condition_landed) { /* landed, don't takeoff, but switch to IDLE mode */ - _mission_item.nav_cmd = NAV_CMD_IDLE; - - _navigator->set_can_loiter_at_sp(false); - - mavlink_log_info(_navigator->get_mavlink_fd(), "landed, switch to IDLE"); + item->nav_cmd = NAV_CMD_IDLE; } else { - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) { /* use current position setpoint */ - _mission_item.lat = pos_sp_triplet->current.lat; - _mission_item.lon = pos_sp_triplet->current.lon; - _mission_item.altitude = pos_sp_triplet->current.alt; + item->lat = pos_sp_triplet->current.lat; + item->lon = pos_sp_triplet->current.lon; + item->altitude = pos_sp_triplet->current.alt; } else { /* use current position */ - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = _navigator->get_global_position()->alt; + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + item->altitude = _navigator->get_global_position()->alt; } - _mission_item.altitude_is_relative = false; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = false; - _mission_item.origin = ORIGIN_ONBOARD; - - _navigator->set_can_loiter_at_sp(true); - mavlink_log_info(_navigator->get_mavlink_fd(), "switch to LOITER"); + item->altitude_is_relative = false; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = 0.0f; + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 4f79238f4..22f42cc31 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -91,11 +91,9 @@ protected: /** * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position */ - // TODO remove argument, get position setpoint from navigator, add to arguments pointer to mission item instead - void set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet); + void set_loiter_item(struct mission_item_s *item); mission_item_s _mission_item; - bool _mission_item_valid; bool _waypoint_position_reached; bool _waypoint_yaw_reached; hrt_abstime _time_first_inside_orbit; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 184ecd365..5dcd8859e 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -103,6 +103,7 @@ public: * Setters */ void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } + void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } /** * Getters @@ -110,6 +111,7 @@ public: struct vehicle_status_s* get_vstatus() { return &_vstatus; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } + struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } Geofence& get_geofence() { return _geofence; } @@ -162,7 +164,7 @@ private: NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ - bool _update_triplet; /**< flags if position SP triplet needs to be published */ + bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 266114e38..bb48db829 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -121,7 +121,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _update_triplet(false), + _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD") { @@ -371,20 +371,21 @@ Navigator::task_main() /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { - _update_triplet = _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i], &_pos_sp_triplet); + _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid */ if (_navigation_mode == nullptr) { + // TODO publish empty sp only once _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; - _update_triplet = true; + _pos_sp_triplet_updated = true; } - if (_update_triplet) { + if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); - _update_triplet = false; + _pos_sp_triplet_updated = false; } perf_end(_loop_perf); diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 6361ea9c8..f43215665 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -40,6 +40,7 @@ */ #include "navigator_mode.h" +#include "navigator.h" NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : SuperBlock(NULL, name), @@ -56,18 +57,17 @@ NavigatorMode::~NavigatorMode() { } -bool -NavigatorMode::run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet) { +void +NavigatorMode::run(bool active) { if (active) { if (_first_run) { /* first run */ _first_run = false; - on_activation(pos_sp_triplet); - return true; + on_activation(); } else { /* periodic updates when active */ - on_active(pos_sp_triplet); + on_active(); } } else { @@ -83,14 +83,13 @@ NavigatorMode::on_inactive() } void -NavigatorMode::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +NavigatorMode::on_activation() { /* invalidate position setpoint by default */ - pos_sp_triplet->current.valid = false; + _navigator->get_position_setpoint_triplet()->current.valid = false; } -bool -NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +NavigatorMode::on_active() { - return false; } diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 5c36af1fe..a7ba79bba 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -66,7 +66,7 @@ public: */ virtual ~NavigatorMode(); - bool run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet); + void run(bool active); /** * This function is called while the mode is inactive @@ -76,15 +76,12 @@ public: /** * This function is called one time when mode become active, poos_sp_triplet must be initialized here */ - virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); /** * This function is called while the mode is active - * - * @param position setpoint triplet to set - * @return true if position setpoint triplet has been changed */ - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); protected: Navigator *_navigator; -- cgit v1.2.3 From 263a2fa9b2258c02bdb0df6746bc289338d63cca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Jun 2014 14:52:09 +0200 Subject: mtecs: add flightpathangle filter --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 19 +++++++----- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 34 +++++++++++++--------- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 7 +++++ src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 +- src/modules/uORB/topics/tecs_status.h | 1 + 6 files changed, 43 insertions(+), 22 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 48c9079a4..fc0a2551c 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -58,6 +58,7 @@ mTecs::mTecs() : _controlEnergyDistribution(this, "PIT", true), _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), + _flightPathAngleLowpass(this, "FPA_LP"), _airspeedLowpass(this, "A_LP"), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), @@ -123,7 +124,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* time measurement */ updateTimeMeasurement(); - /* Filter arispeed */ + /* Filter airspeed */ float airspeedFiltered = _airspeedLowpass.update(airspeed); /* calculate longitudinal acceleration setpoint from airspeed setpoint*/ @@ -138,8 +139,6 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng } /* Write part of the status message */ - _status.flightPathAngleSp = flightPathAngleSp; - _status.flightPathAngle = flightPathAngle; _status.airspeedSp = airspeedSp; _status.airspeed = airspeed; _status.airspeedFiltered = airspeedFiltered; @@ -164,8 +163,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* update parameters first */ updateParams(); + /* Filter flightpathangle */ + float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle); + /* calculate values (energies) */ - float flightPathAngleError = flightPathAngleSp - flightPathAngle; + float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered; float airspeedDerivative = 0.0f; if(_airspeedDerivative.getDt() > 0.0f) { airspeedDerivative = _airspeedDerivative.update(airspeedFiltered); @@ -175,12 +177,12 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G; float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm; - float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm; + float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm; float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError; float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp; float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate; - float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm; + float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm; float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError; float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp; float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; @@ -202,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); if (mode == TECS_MODE_TAKEOFF) { - outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector + outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; } else if (mode == TECS_MODE_LAND) { // only limit pitch but do not limit throttle @@ -221,6 +223,9 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); /* Write part of the status message */ + _status.flightPathAngleSp = flightPathAngleSp; + _status.flightPathAngle = flightPathAngle; + _status.flightPathAngleFiltered = flightPathAngleFiltered; _status.airspeedDerivativeSp = airspeedDerivativeSp; _status.airspeedDerivative = airspeedDerivative; _status.totalEnergyRateSp = totalEnergyRateSp; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index c102f5dee..efa89a5d3 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -104,12 +104,17 @@ protected: uORB::Publication _status; /**< publish internal values for logging */ /* control blocks */ - BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ - BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ - BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight path angle setpoint */ - BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */ + BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output + is throttle */ + BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: + output is pitch */ + BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight + path angle setpoint */ + BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration + setpoint */ /* Other calculation Blocks */ + control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ @@ -118,21 +123,22 @@ protected: float _pitchSp; /**< Pitch Setpoint from -pi to pi */ /* Output Limits in special modes */ - BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ - BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ - BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in last phase)*/ - BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ + BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in + last phase)*/ + BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ /* Time measurements */ - hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ + hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ - bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ - bool _dtCalculated; /**< True if dt has been calculated in this iteration */ + bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + bool _dtCalculated; /**< True if dt has been calculated in this iteration */ int _counter; - bool _debug; ///< Set true to enable debug output + bool _debug; ///< Set true to enable debug output static void debug_print(const char *fmt, va_list args); diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index bbbb8f9db..5b9238780 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); */ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); +/** + * Lowpass (cutoff freq.) for the flight path angle + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f); + /** * P gain for the altitude control * Maps the altitude error to the flight path angle setpoint diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0813bf7b0..ab9c1639c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1507,6 +1507,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; + log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5645ebcf1..a631317df 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -353,6 +353,7 @@ struct log_TECS_s { float altitude; float flightPathAngleSp; float flightPathAngle; + float flightPathAngleFiltered; float airspeedSp; float airspeed; float airspeedFiltered; @@ -431,7 +432,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index 05310e906..c4d0c1874 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -68,6 +68,7 @@ struct tecs_status_s { float altitude; float flightPathAngleSp; float flightPathAngle; + float flightPathAngleFiltered; float airspeedSp; float airspeed; float airspeedFiltered; -- cgit v1.2.3 From adf230ce4efe30087e751f143dab1df33a7d3b70 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Jun 2014 15:35:34 +0200 Subject: navigator: more API changes, duplicate code removed --- src/modules/navigator/mission.cpp | 166 ++++++++++++++------------------ src/modules/navigator/mission.h | 10 +- src/modules/navigator/mission_block.cpp | 5 +- src/modules/navigator/mission_block.h | 2 +- src/modules/navigator/rtl.cpp | 25 ++--- src/modules/navigator/rtl.h | 6 +- 6 files changed, 95 insertions(+), 119 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 1e86b1b6c..fbfd1259d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -225,29 +225,27 @@ Mission::set_mission_items() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - set_previous_pos_setpoint(pos_sp_triplet); + /* set previous position setpoint to current */ + set_previous_pos_setpoint(); /* try setting onboard mission item */ - if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { + if (set_mission_item(true, false, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: onboard mission running"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running"); } _mission_type = MISSION_TYPE_ONBOARD; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); /* try setting offboard mission item */ - } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { + } else if (set_mission_item(false, false, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: offboard mission running"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); } _mission_type = MISSION_TYPE_OFFBOARD; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); } else { + /* no mission available, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: mission finished"); @@ -257,126 +255,108 @@ Mission::set_mission_items() } _mission_type = MISSION_TYPE_NONE; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); - + /* set loiter mission item */ set_loiter_item(&_mission_item); + /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + reset_mission_item_reached(); report_mission_finished(); + + _navigator->set_position_setpoint_triplet_updated(); + return; } -} -bool -Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) -{ - /* make sure param is up to date */ - updateParams(); - if (_param_onboard_enabled.get() > 0 && - _current_onboard_mission_index >= 0&& - _current_onboard_mission_index < (int)_onboard_mission.count) { - struct mission_item_s new_mission_item; - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, - &new_mission_item)) { - /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); + /* new current mission item set */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); + + if (_mission_type == MISSION_TYPE_OFFBOARD) { + report_current_offboard_mission_item(); + } + // TODO: report onboard mission item somehow - reset_mission_item_reached(); + /* try to read next mission item */ + struct mission_item_s mission_item_next; + if (!set_mission_item(_mission_type == MISSION_TYPE_ONBOARD, true, &mission_item_next)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); - /* TODO: report this somehow */ - memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); - return true; - } + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; } - return false; + + _navigator->set_position_setpoint_triplet_updated(); } bool -Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) +Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item) { - if (_current_offboard_mission_index >= 0 && - _current_offboard_mission_index < (int)_offboard_mission.count) { - dm_item_t dm_current; - if (_offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - struct mission_item_s new_mission_item; - if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) { - /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); + /* make sure param is up to date */ + updateParams(); - reset_mission_item_reached(); + /* select onboard/offboard mission */ + int *mission_index_ptr; + struct mission_s *mission; + dm_item_t dm_item; + int mission_index_next; - report_current_offboard_mission_item(); - memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); - return true; + if (onboard) { + /* onboard mission */ + if (!_param_onboard_enabled.get()) { + return false; } - } - return false; -} -void -Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) -{ - int next_temp_mission_index = _onboard_mission.current_index + 1; + mission_index_next = _current_onboard_mission_index + 1; + mission_index_ptr = next_item ? &mission_index_next : &_current_onboard_mission_index; - /* try if there is a next onboard mission */ - if (_onboard_mission.current_index >= 0 && - next_temp_mission_index < (int)_onboard_mission.count) { - struct mission_item_s new_mission_item; - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { - /* convert next mission item to position setpoint */ - mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - return; - } - } + mission = &_onboard_mission; - /* give up */ - next_pos_sp->valid = false; - return; -} + dm_item = DM_KEY_WAYPOINTS_ONBOARD; + + } else { + /* offboard mission */ + mission_index_next = _current_offboard_mission_index + 1; + mission_index_ptr = next_item ? &mission_index_next : &_current_offboard_mission_index; + + mission = &_offboard_mission; -void -Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) -{ - /* try if there is a next offboard mission */ - int next_temp_mission_index = _offboard_mission.current_index + 1; - warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); - if (_offboard_mission.current_index >= 0 && - next_temp_mission_index < (int)_offboard_mission.count) { - dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item = DM_KEY_WAYPOINTS_OFFBOARD_1; } + } + + if (*mission_index_ptr >= 0 && *mission_index_ptr < (int)mission->count) { struct mission_item_s new_mission_item; - if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) { - /* convert next mission item to position setpoint */ - mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); - return; + + if (read_mission_item(dm_item, true, mission_index_ptr, &new_mission_item)) { + memcpy(&mission_item, &new_mission_item, sizeof(struct mission_item_s)); + return true; } } - /* give up */ - next_pos_sp->valid = false; - return; + return false; } bool -Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, +Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index_ptr, struct mission_item_s *new_mission_item) { /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ - for (int i=0; i<10; i++) { + for (int i = 0; i < 10; i++) { const ssize_t len = sizeof(struct mission_item_s); /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { + if (dm_read(dm_item, *mission_index_ptr, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_log_critical(_navigator->get_mavlink_fd(), "#audio: ERROR waypoint could not be read"); @@ -394,7 +374,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio if (is_current) { (new_mission_item->do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ @@ -405,12 +385,12 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; + *mission_index_ptr = new_mission_item->do_jump_mission_index; } else { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: DO JUMP repetitions completed"); /* no more DO_JUMPS, therefore just try to continue with next mission item */ - (*mission_index)++; + (*mission_index_ptr)++; } } else { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index ac293990f..7d07860de 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -97,16 +97,10 @@ private: void set_mission_items(); /** - * Try to set the current position setpoint from an onboard mission item + * Try to set given mission item from an offboard/onboard mission item * @return true if mission item successfully set */ - bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp); - - /** - * Try to set the current position setpoint from an offboard mission item - * @return true if mission item successfully set - */ - bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp); + bool set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item); /** * Try to set the next position setpoint from an onboard mission item diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index bf3c934a6..cbf7ac987 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -202,9 +202,10 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite } void -MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet) +MissionBlock::set_previous_pos_setpoint() { - /* reuse current setpoint as previous setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + if (pos_sp_triplet->current.valid) { memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s)); } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 22f42cc31..adf50bc39 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -86,7 +86,7 @@ protected: /** * Set previous position setpoint to current setpoint */ - void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet); + void set_previous_pos_setpoint(); /** * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 7cf6bb1a8..6d7afcf4b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -82,7 +82,7 @@ RTL::on_inactive() } void -RTL::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +RTL::on_activation() { /* decide where to enter the RTL procedure when we switch into it */ if (_rtl_state == RTL_STATE_NONE) { @@ -105,28 +105,27 @@ RTL::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) } } - set_rtl_item(pos_sp_triplet); + set_rtl_item(); } -bool -RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +RTL::on_active() { if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { advance_rtl(); - set_rtl_item(pos_sp_triplet); - return true; + set_rtl_item(); } - - return false; } void -RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) +RTL::set_rtl_item() { + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + /* make sure we have the latest params */ updateParams(); - set_previous_pos_setpoint(pos_sp_triplet); + set_previous_pos_setpoint(); _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { @@ -273,11 +272,13 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) break; } + reset_mission_item_reached(); + /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - reset_mission_item_reached(); - pos_sp_triplet->current.valid = true; pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); } void diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9c8b3fdfc..5928f1f07 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -63,15 +63,15 @@ public: virtual void on_inactive(); - virtual void on_activation(position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); - virtual bool on_active(position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_active(); private: /** * Set the RTL item */ - void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet); + void set_rtl_item(); /** * Move to next RTL item -- cgit v1.2.3 From 72071cdcd39cae6be8533e67d01cbc7533ca6cb1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 19:22:22 +0200 Subject: Fix failed merge --- src/modules/mavlink/mavlink_main.h | 54 -------------------------------------- 1 file changed, 54 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 18141972a..91d79057c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -138,14 +138,6 @@ public: bool get_forwarding_on() { return _forwarding_on; } -<<<<<<< HEAD -======= - /** - * Handle waypoint related messages. - */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 - static int start_helper(int argc, char *argv[]); /** @@ -165,15 +157,11 @@ public: */ int set_hil_enabled(bool hil_enabled); -<<<<<<< HEAD void send_message(const mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg); MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); -======= - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 int get_instance_id(); @@ -228,7 +216,6 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; -<<<<<<< HEAD MavlinkMissionManager *_mission_manager; orb_advert_t _mission_pub; @@ -236,32 +223,12 @@ private: MAVLINK_MODE _mode; mavlink_channel_t _channel; -======= - orb_advert_t _mission_pub; - struct mission_s mission; - MAVLINK_MODE _mode; - - uint8_t _mavlink_wpm_comp_id; - mavlink_channel_t _channel; ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; pthread_t _receive_thread; -<<<<<<< HEAD - bool _verbose; - bool _forwarding_on; - bool _passing_on; - int _uart_fd; - int _baudrate; - int _datarate; -======= - /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; - bool _verbose; bool _forwarding_on; bool _passing_on; @@ -269,7 +236,6 @@ private: int _uart_fd; int _baudrate; int _datarate; ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 /** * If the queue index is not at 0, the queue sending @@ -347,26 +313,9 @@ private: void mavlink_update_system(); -<<<<<<< HEAD uint8_t get_system_id(); uint8_t get_component_id(); -======= - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); - void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); - void mavlink_wpm_send_waypoint_current(uint16_t seq); - void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); - void mavlink_wpm_init(mavlink_wpm_storage *state); - int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); - void publish_mission(); - - void mavlink_missionlib_send_message(mavlink_message_t *msg); - int mavlink_missionlib_send_gcs_string(const char *string); ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); @@ -381,11 +330,8 @@ private: int message_buffer_is_empty(); -<<<<<<< HEAD bool message_buffer_write(const void *ptr, int size); -======= ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); -- cgit v1.2.3 From d5b5dcef24a7f5c7b2ed98081dd69a05f8d46959 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 12:01:43 -0700 Subject: Fix bugs found through compiler warnings --- src/drivers/mkblctrl/mkblctrl.cpp | 8 ++++---- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/roboclaw/RoboClaw.cpp | 10 ++++++++-- src/modules/commander/commander.cpp | 1 + src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- src/modules/ekf_att_pos_estimator/estimator.h | 2 +- src/modules/gpio_led/gpio_led.c | 3 ++- src/modules/px4iofirmware/sbus.c | 10 ++++++++-- src/systemcmds/mtd/mtd.c | 8 ++++++-- src/systemcmds/tests/test_mixer.cpp | 1 + 10 files changed, 33 insertions(+), 14 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 5954c40da..493678ae5 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -131,8 +131,8 @@ public: int set_motor_count(unsigned count); int set_motor_test(bool motortest); int set_overrideSecurityChecks(bool overrideSecurityChecks); - int set_px4mode(int px4mode); - int set_frametype(int frametype); + void set_px4mode(int px4mode); + void set_frametype(int frametype); unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); private: @@ -330,13 +330,13 @@ MK::set_update_rate(unsigned rate) return OK; } -int +void MK::set_px4mode(int px4mode) { _px4mode = px4mode; } -int +void MK::set_frametype(int frametype) { _frametype = frametype; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8a4bfa18c..84ea9a3bc 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -741,7 +741,7 @@ PX4FMU::task_main() } for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs > 0) { + if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index dd5e4d3e0..b0a8527c2 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -182,7 +182,10 @@ float RoboClaw::getMotorPosition(e_motor motor) return _motor1Position; } else if (motor == MOTOR_2) { return _motor2Position; - } + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } } float RoboClaw::getMotorSpeed(e_motor motor) @@ -191,7 +194,10 @@ float RoboClaw::getMotorSpeed(e_motor motor) return _motor1Speed; } else if (motor == MOTOR_2) { return _motor2Speed; - } + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } } int RoboClaw::setMotorSpeed(e_motor motor, float value) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1535967b1..3cd250458 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -603,6 +603,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // XXX TODO } + return true; } int commander_thread_main(int argc, char *argv[]) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 1320b4668..f8b9e9fbd 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1986,7 +1986,7 @@ void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt posNED[2] = -(hgt - hgtRef); } -void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void AttPosEKF::calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef) { lat = latRef + posNED[0] * earthRadiusInv; lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e821089f2..69d8cfce8 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -301,7 +301,7 @@ static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, flo static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); -static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef); static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 839a7890b..1fc6d1295 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -189,7 +189,8 @@ int gpio_led_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; - warnx("stop"); + // FIXME: Is this correct? changed from warnx + errx(0, "stop"); } else { errx(1, "not running"); diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 0e7dc621c..8ed69678c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -119,13 +119,19 @@ sbus_init(const char *device) bool sbus1_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'A', 1); + // FIXME: I assume this was previously NYI? + ssize_t cBytes = num_values * sizeof(values[0]); + ssize_t cBytesWritten = write(sbus_fd, values, cBytes); + return cBytesWritten == cBytes; } bool sbus2_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'B', 1); + // FIXME: I assume this was previously NYI? + ssize_t cBytes = num_values * sizeof(values[0]); + ssize_t cBytesWritten = write(sbus_fd, values, cBytes); + return cBytesWritten == cBytes; } bool diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index a57eaafe7..8b43e54da 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -193,8 +193,12 @@ ramtron_attach(void) errx(1, "failed to initialize mtd driver"); int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); - if (ret != OK) - warnx(1, "failed to set bus speed"); + if (ret != OK) { + // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried + // that but setting the bug speed does fail all the time. Which was then exiting and the board would + // not run correctly. So changed to warnx. + warnx("failed to set bus speed"); + } attached = true; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index df382e2c6..dc2c20e79 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -372,6 +372,7 @@ int test_mixer(int argc, char *argv[]) } warnx("SUCCESS: No errors in mixer test"); + return 0; } static int -- cgit v1.2.3 From f0558a97ddbbd275022766603a866e86316bbb5b Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 12:53:59 -0700 Subject: Use same warnx/exit pattern --- src/modules/gpio_led/gpio_led.c | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 1fc6d1295..def958c95 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -181,17 +181,13 @@ int gpio_led_main(int argc, char *argv[]) } else { gpio_led_started = true; warnx("start, using pin: %s", pin_name); + exit(0); } - - exit(0); - - } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; - // FIXME: Is this correct? changed from warnx - errx(0, "stop"); - + warnx("stop"); + exit(0); } else { errx(1, "not running"); } -- cgit v1.2.3 From 367613a1b57f2c52e97958ab40426fc77245383a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 13:32:58 -0700 Subject: Fix tabbing --- src/drivers/mkblctrl/mkblctrl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 493678ae5..1c81910f6 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -131,8 +131,8 @@ public: int set_motor_count(unsigned count); int set_motor_test(bool motortest); int set_overrideSecurityChecks(bool overrideSecurityChecks); - void set_px4mode(int px4mode); - void set_frametype(int frametype); + void set_px4mode(int px4mode); + void set_frametype(int frametype); unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); private: -- cgit v1.2.3 From 33c20e15a2246e9249bbb8183758604fdc231183 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 13:35:15 -0700 Subject: Backing out sbus changes --- src/modules/px4iofirmware/sbus.c | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 8ed69678c..0e7dc621c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -119,19 +119,13 @@ sbus_init(const char *device) bool sbus1_output(uint16_t *values, uint16_t num_values) { - // FIXME: I assume this was previously NYI? - ssize_t cBytes = num_values * sizeof(values[0]); - ssize_t cBytesWritten = write(sbus_fd, values, cBytes); - return cBytesWritten == cBytes; + write(sbus_fd, 'A', 1); } bool sbus2_output(uint16_t *values, uint16_t num_values) { - // FIXME: I assume this was previously NYI? - ssize_t cBytes = num_values * sizeof(values[0]); - ssize_t cBytesWritten = write(sbus_fd, values, cBytes); - return cBytesWritten == cBytes; + write(sbus_fd, 'B', 1); } bool -- cgit v1.2.3 From f3af2f9f624bb3111b90d8b6d428d99de8ad3a8e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 13:47:56 -0700 Subject: Fix tabbing to 8 spaces, keep tab --- src/modules/mavlink/mavlink_ftp.cpp | 84 ++++++++++++++++++------------------- src/modules/mavlink/mavlink_ftp.h | 4 +- 2 files changed, 44 insertions(+), 44 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index f1436efb8..ca846a465 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -127,7 +127,7 @@ MavlinkFTP::_worker(Request *req) break; case kCmdTerminate: - errorCode = _workTerminate(req); + errorCode = _workTerminate(req); break; case kCmdReset: @@ -222,12 +222,12 @@ MavlinkFTP::_workList(Request *req) // no more entries? if (result == nullptr) { - if (hdr->offset != 0 && offset == 0) { - // User is requesting subsequent dir entries but there were none. This means the user asked - // to seek past EOF. - errorCode = kErrEOF; - } - // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that + if (hdr->offset != 0 && offset == 0) { + // User is requesting subsequent dir entries but there were none. This means the user asked + // to seek past EOF. + errorCode = kErrEOF; + } + // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that break; } @@ -266,18 +266,18 @@ MavlinkFTP::_workOpen(Request *req, bool create) { auto hdr = req->header(); - int session_index = _findUnusedSession(); + int session_index = _findUnusedSession(); if (session_index < 0) { return kErrNoSession; } int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; - int fd = ::open(req->dataAsCString(), oflag); + int fd = ::open(req->dataAsCString(), oflag); if (fd < 0) { return create ? kErrPerm : kErrNotFile; } - _session_fds[session_index] = fd; + _session_fds[session_index] = fd; hdr->session = session_index; hdr->size = 0; @@ -290,28 +290,28 @@ MavlinkFTP::_workRead(Request *req) { auto hdr = req->header(); - int session_index = hdr->session; - - if (!_validSession(session_index)) { + int session_index = hdr->session; + + if (!_validSession(session_index)) { return kErrNoSession; - } - - // Seek to the specified position - printf("Seek %d\n", hdr->offset); + } + + // Seek to the specified position + printf("Seek %d\n", hdr->offset); if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { - // Unable to see to the specified location + // Unable to see to the specified location return kErrEOF; } - + int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); if (bytes_read < 0) { - // Negative return indicates error other than eof + // Negative return indicates error other than eof return kErrIO; } - - printf("Read success %d\n", bytes_read); + + printf("Read success %d\n", bytes_read); hdr->size = bytes_read; - + return kErrNone; } @@ -357,26 +357,26 @@ MavlinkFTP::_workTerminate(Request *req) { auto hdr = req->header(); - if (!_validSession(hdr->session)) { + if (!_validSession(hdr->session)) { return kErrNoSession; - } + } - ::close(_session_fds[hdr->session]); + ::close(_session_fds[hdr->session]); - return kErrNone; + return kErrNone; } MavlinkFTP::ErrorCode MavlinkFTP::_workReset(void) { - for (size_t i=0; i Date: Sun, 29 Jun 2014 13:55:55 -0700 Subject: More tab fixes --- src/drivers/roboclaw/RoboClaw.cpp | 8 ++++---- src/modules/commander/commander.cpp | 2 +- src/modules/gpio_led/gpio_led.c | 4 ++-- src/systemcmds/mtd/mtd.c | 12 ++++++------ src/systemcmds/tests/test_mixer.cpp | 2 +- 5 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index b0a8527c2..fdaa7f27b 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -183,8 +183,8 @@ float RoboClaw::getMotorPosition(e_motor motor) } else if (motor == MOTOR_2) { return _motor2Position; } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; } } @@ -195,8 +195,8 @@ float RoboClaw::getMotorSpeed(e_motor motor) } else if (motor == MOTOR_2) { return _motor2Speed; } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3cd250458..aa243d79b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -603,7 +603,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // XXX TODO } - return true; + return true; } int commander_thread_main(int argc, char *argv[]) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index def958c95..7758faed7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -181,13 +181,13 @@ int gpio_led_main(int argc, char *argv[]) } else { gpio_led_started = true; warnx("start, using pin: %s", pin_name); - exit(0); + exit(0); } } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; warnx("stop"); - exit(0); + exit(0); } else { errx(1, "not running"); } diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 8b43e54da..fcc9b8366 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -193,12 +193,12 @@ ramtron_attach(void) errx(1, "failed to initialize mtd driver"); int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); - if (ret != OK) { - // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried - // that but setting the bug speed does fail all the time. Which was then exiting and the board would - // not run correctly. So changed to warnx. - warnx("failed to set bus speed"); - } + if (ret != OK) { + // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried + // that but setting the bug speed does fail all the time. Which was then exiting and the board would + // not run correctly. So changed to warnx. + warnx("failed to set bus speed"); + } attached = true; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index dc2c20e79..0b826b826 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -372,7 +372,7 @@ int test_mixer(int argc, char *argv[]) } warnx("SUCCESS: No errors in mixer test"); - return 0; + return 0; } static int -- cgit v1.2.3 From 43b9d96cf4306bc472d16956f5437dbef7c630c0 Mon Sep 17 00:00:00 2001 From: Kynos Date: Mon, 30 Jun 2014 01:55:40 +0200 Subject: Merged upstream master, add missing change --- src/modules/mavlink/mavlink_messages.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 472e2810d..06c2e737e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -669,16 +669,16 @@ protected: if (gps_sub->update(&gps_time, &gps)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph), - cm_uint16_from_m_float(gps->epv), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_used); + gps.timestamp_position, + gps.fix_type, + gps.lat, + gps.lon, + gps.alt, + cm_uint16_from_m_float(gps.eph), + cm_uint16_from_m_float(gps.epv), + gps.vel_m_s * 100.0f, + _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps.satellites_used); } } }; -- cgit v1.2.3 From 92adbe9216c96c53d1baa4eb1e14b4ede272c080 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 17:47:24 -0700 Subject: Fix compiler warnings --- src/drivers/boards/px4fmu-v2/px4fmu2_led.c | 2 +- src/drivers/ets_airspeed/ets_airspeed.cpp | 4 +-- src/drivers/gps/gps.cpp | 6 ++-- src/drivers/hmc5883/hmc5883.cpp | 4 +-- src/drivers/mkblctrl/mkblctrl.cpp | 19 ++++------ src/drivers/px4fmu/fmu.cpp | 12 +++---- src/drivers/px4io/px4io_serial.cpp | 2 +- src/drivers/sf0x/sf0x.cpp | 2 +- src/examples/px4_simple_app/px4_simple_app.c | 1 + src/lib/geo/geo.c | 33 +++++++++-------- src/lib/geo_lookup/geo_mag_declination.c | 31 +++++++--------- .../attitude_estimator_ekf_params.c | 2 +- .../attitude_estimator_so3_main.cpp | 21 +++-------- .../commander/accelerometer_calibration.cpp | 6 ++-- src/modules/commander/calibration_routines.cpp | 2 +- src/modules/dataman/dataman.c | 3 +- .../ekf_att_pos_estimator/estimator_23states.h | 2 +- src/modules/fw_pos_control_l1/landingslope.cpp | 16 ++++----- src/modules/fw_pos_control_l1/landingslope.h | 8 ++--- src/modules/mavlink/mavlink_main.cpp | 6 ++-- src/modules/mavlink/mavlink_main.h | 4 +-- src/modules/mavlink/mavlink_orb_subscription.cpp | 6 ++-- src/modules/mavlink/mavlink_stream.cpp | 6 +++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 +- .../position_estimator_inav_main.c | 41 ++++++++++------------ src/modules/px4iofirmware/protocol.h | 2 +- src/modules/px4iofirmware/px4io.c | 5 ++- src/modules/px4iofirmware/registers.c | 3 +- src/modules/sensors/sensors.cpp | 2 +- src/modules/systemlib/hx_stream.c | 2 +- src/modules/systemlib/mixer/mixer_multirotor.cpp | 4 +-- src/modules/systemlib/otp.c | 2 +- src/modules/systemlib/param/param.c | 2 -- src/modules/systemlib/pwm_limit/pwm_limit.c | 1 - src/systemcmds/mtd/mtd.c | 30 ++++++++-------- src/systemcmds/param/param.c | 4 +-- src/systemcmds/tests/test_conv.cpp | 2 +- src/systemcmds/tests/test_file.c | 7 ++-- src/systemcmds/tests/test_file2.c | 10 +++--- src/systemcmds/tests/test_float.c | 10 +++--- src/systemcmds/tests/test_mathlib.cpp | 4 --- src/systemcmds/tests/test_mixer.cpp | 33 ++++++----------- src/systemcmds/tests/test_ppm_loopback.c | 1 - src/systemcmds/tests/test_rc.c | 1 + src/systemcmds/tests/test_servo.c | 1 + 45 files changed, 162 insertions(+), 206 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c index a856ccb02..3c05bfa46 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -54,7 +54,7 @@ * CONFIG_ARCH_LEDS configuration switch. */ __BEGIN_DECLS -extern void led_init(); +extern void led_init(void); extern void led_on(int led); extern void led_off(int led); extern void led_toggle(int led); diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 2de7063ea..1a7e068fe 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -364,7 +364,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -389,7 +389,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); } /* reset the sensor polling to its default rate */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 8560716e9..dd505abdb 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -357,7 +357,7 @@ GPS::task_main() } if (!_healthy) { - char *mode_str = "unknown"; + const char *mode_str = "unknown"; switch (_mode) { case GPS_DRIVER_MODE_UBX: @@ -449,7 +449,7 @@ GPS::print_info() if (_report.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type, - _report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f); + _report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0d); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); @@ -578,7 +578,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char *device_name = GPS_DEFAULT_UART_PORT; + const char *device_name = GPS_DEFAULT_UART_PORT; bool fake_gps = false; /* diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index fddba806e..b7b368a5e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -326,9 +326,9 @@ HMC5883::HMC5883(int bus) : _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _collect_phase(false), + _class_instance(-1), _mag_topic(-1), _subsystem_pub(-1), - _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -1228,7 +1228,7 @@ HMC5883::print_info() printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, - (double)1.0/_range_scale, (double)_range_ga); + (double)(1.0f/_range_scale), (double)_range_ga); _reports->print_info("report queue"); } diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1c81910f6..3996b76a6 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -222,15 +222,15 @@ MK::MK(int bus, const char *_device_path) : _task(-1), _t_actuators(-1), _t_actuator_armed(-1), + _motor(-1), + _px4mode(MAPPING_MK), + _frametype(FRAME_PLUS), _t_outputs(0), _t_esc_status(0), _num_outputs(0), + _primary_pwm_device(false), _motortest(false), _overrideSecurityChecks(false), - _motor(-1), - _px4mode(MAPPING_MK), - _frametype(FRAME_PLUS), - _primary_pwm_device(false), _task_should_exit(false), _armed(false), _mixers(nullptr) @@ -440,9 +440,6 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax) void MK::task_main() { - long update_rate_in_us = 0; - float tmpVal = 0; - /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. @@ -483,7 +480,6 @@ MK::task_main() /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); - update_rate_in_us = long(1000000 / _update_rate); /* reject faster than 500 Hz updates */ if (update_rate_in_ms < 2) { @@ -735,7 +731,6 @@ MK::mk_servo_set(unsigned int chan, short val) _retries = 0; uint8_t result[3] = { 0, 0, 0 }; uint8_t msg[2] = { 0, 0 }; - uint8_t rod = 0; uint8_t bytesToSendBL2 = 2; tmpVal = val; @@ -824,7 +819,7 @@ MK::mk_servo_set(unsigned int chan, short val) if (debugCounter == 2000) { debugCounter = 0; - for (int i = 0; i < _num_outputs; i++) { + for (unsigned int i = 0; i < _num_outputs; i++) { if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) { fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); } @@ -1169,7 +1164,7 @@ mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int fr } int -mk_start(unsigned motors, char *device_path) +mk_start(unsigned motors, const char *device_path) { int ret; @@ -1228,7 +1223,7 @@ mkblctrl_main(int argc, char *argv[]) bool overrideSecurityChecks = false; bool showHelp = false; bool newMode = false; - char *devicepath = ""; + const char *devicepath = ""; /* * optional parameters diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 84ea9a3bc..0a4635728 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -240,8 +240,6 @@ PX4FMU::PX4FMU() : _pwm_alt_rate_channels(0), _current_update_rate(0), _task(-1), - _control_subs({-1}), - _poll_fds_num(0), _armed_sub(-1), _outputs_pub(-1), _num_outputs(0), @@ -252,10 +250,12 @@ PX4FMU::PX4FMU() : _mixers(nullptr), _groups_required(0), _groups_subscribed(0), - _failsafe_pwm({0}), - _disarmed_pwm({0}), - _num_failsafe_set(0), - _num_disarmed_set(0) + _control_subs{-1}, + _poll_fds_num(0), + _failsafe_pwm{0}, + _disarmed_pwm{0}, + _num_failsafe_set(0), + _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 3b210ac59..c39494fb0 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt() if (_rx_dma_status == _dma_status_waiting) { /* verify that the received packet is complete */ - int length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { perf_count(_pc_badidle); diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 9109af14f..bca1715fa 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -616,7 +616,7 @@ SF0X::collect() } } - debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO")); + debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); /* done with this chunk, resetting - even if invalid */ _linebuf_index = 0; diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 44e6dc7f3..4e9f099ed 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9a24ff50e..212e33ff8 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -82,8 +82,8 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { - float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; - float y_rad = y / CONSTANTS_RADIUS_OF_EARTH; + double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; + double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; double c = sqrtf(x_rad * x_rad + y_rad * y_rad); double sin_c = sin(c); double cos_c = cos(c); @@ -146,7 +146,6 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double double lat_next_rad = lat_next * M_DEG_TO_RAD; double lon_next_rad = lon_next * M_DEG_TO_RAD; - double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ @@ -174,8 +173,8 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; - *lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; - *lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; + *lat_res = (lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; + *lon_res = (lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; } // Additional functions - @author Doug Weibel @@ -197,7 +196,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } + if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); @@ -212,7 +211,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d } dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - crosstrack_error->distance = (dist_to_end) * sin(bearing_diff); + crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff); if (sin(bearing_diff) >= 0) { crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); @@ -248,7 +247,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; } + if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; } if (arc_sweep >= 0) { @@ -296,14 +295,14 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do // as this function generally will not be called repeatedly when we are out of the sector. // TO DO - this is messed up and won't compile - float start_disp_x = radius * sin(arc_start_bearing); - float start_disp_y = radius * cos(arc_start_bearing); - float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0d; - float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; - float lon_end = lon_now + end_disp_x / 111111.0d; - float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; + float start_disp_x = radius * sinf(arc_start_bearing); + float start_disp_y = radius * cosf(arc_start_bearing); + float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep)); + float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep)); + float lon_start = lon_now + start_disp_x / 111111.0f; + float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f; + float lon_end = lon_now + end_disp_x / 111111.0f; + float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f; float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); @@ -337,7 +336,7 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now double d_lat = x_rad - current_x_rad; double d_lon = y_rad - current_y_rad; - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad); double c = 2 * atan2(sqrt(a), sqrt(1 - a)); float dxy = CONSTANTS_RADIUS_OF_EARTH * c; diff --git a/src/lib/geo_lookup/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c index 09eac38f4..c41d52606 100644 --- a/src/lib/geo_lookup/geo_mag_declination.c +++ b/src/lib/geo_lookup/geo_mag_declination.c @@ -54,24 +54,19 @@ static const int8_t declination_table[13][37] = \ { - 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \ - -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \ - -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \ - 29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \ - -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \ - 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \ - 10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \ - -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \ - -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \ - 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \ - 7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \ - 0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \ - 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \ - 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \ - -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \ - 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \ - 0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \ - 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 + { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, + { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, + { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, + { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, + { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, + { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, + { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, + { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, + { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, + { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, + { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, + { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, + { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, }; static float get_lookup_table_val(unsigned lat, unsigned lon); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 49a892609..bc0e3b93a 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -101,7 +101,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r3, &(p->r[3])); param_get(h->mag_decl, &(p->mag_decl)); - p->mag_decl *= M_PI / 180.0f; + p->mag_decl *= M_PI_F / 180.0f; param_get(h->acc_comp, &(p->acc_comp)); diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e55b01160..e49027e47 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -392,8 +392,6 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl */ int attitude_estimator_so3_thread_main(int argc, char *argv[]) { - const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - //! Time constant float dt = 0.005f; @@ -438,11 +436,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; - int printcounter = 0; thread_running = true; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; // XXX write this out to perf regs /* keep track of sensor updates */ @@ -513,7 +509,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; - warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]); + warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]); } } else { @@ -523,12 +519,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { - update_vect[0] = 1; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -538,8 +531,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { - update_vect[1] = 1; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } @@ -549,8 +540,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { - update_vect[2] = 1; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } @@ -569,8 +558,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) continue; } - uint64_t timing_start = hrt_absolute_time(); - // NOTE : Accelerometer is reversed. // Because proper mount of PX4 will give you a reversed accelerometer readings. NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], @@ -609,9 +596,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* due to inputs or numerical failure the output is invalid, skip it */ // Due to inputs or numerical failure the output is invalid warnx("infinite euler angles, rotation matrix:"); - warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]); - warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]); - warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]); // Don't publish anything continue; } diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7180048ff..24da452b1 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -158,6 +158,8 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { + int fd; + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); struct accel_scale accel_scale = { @@ -172,7 +174,7 @@ int do_accel_calibration(int mavlink_fd) int res = OK; /* reset all offsets to zero and all scales to one */ - int fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, 0); res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); @@ -223,7 +225,7 @@ int do_accel_calibration(int mavlink_fd) if (res == OK) { /* apply new scaling and offsets */ - int fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, 0); res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index be38ea104..9d79124e7 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -170,7 +170,7 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], float aA, aB, aC, nA, nB, nC, dA, dB, dC; //Iterate N times, ignore stop condition. - int n = 0; + unsigned int n = 0; while (n < max_iterations) { n++; diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 1a65313e8..406293bda 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -50,6 +50,7 @@ #include #include "dataman.h" +#include /** * data manager app start / stop handling function @@ -187,7 +188,7 @@ create_work_item(void) if (item) { item->first = 1; lock_queue(&g_free_q); - for (int i = 1; i < k_work_item_allocation_chunk_size; i++) { + for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) { (item + i)->first = 0; sq_addfirst(&(item + i)->link, &(g_free_q.q)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 7aad849f9..15ceb57c0 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -234,7 +234,7 @@ int RecallStates(float *statesForFusion, uint64_t msec); void ResetStoredStates(); -void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); +void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index 8ce465fe8..42e00da05 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -46,16 +46,16 @@ #include #include -void Landingslope::update(float landing_slope_angle_rad, - float flare_relative_alt, - float motor_lim_relative_alt, - float H1_virt) +void Landingslope::update(float landing_slope_angle_rad_new, + float flare_relative_alt_new, + float motor_lim_relative_alt_new, + float H1_virt_new) { - _landing_slope_angle_rad = landing_slope_angle_rad; - _flare_relative_alt = flare_relative_alt; - _motor_lim_relative_alt = motor_lim_relative_alt; - _H1_virt = H1_virt; + _landing_slope_angle_rad = landing_slope_angle_rad_new; + _flare_relative_alt = flare_relative_alt_new; + _motor_lim_relative_alt = motor_lim_relative_alt_new; + _H1_virt = H1_virt_new; calculateSlopeValues(); } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index b54fd501c..a5975ad43 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -123,10 +123,10 @@ public: float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); - void update(float landing_slope_angle_rad, - float flare_relative_alt, - float motor_lim_relative_alt, - float H1_virt); + void update(float landing_slope_angle_rad_new, + float flare_relative_alt_new, + float motor_lim_relative_alt_new, + float H1_virt_new); inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;} diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9a5e31ef4..43acee96f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2266,13 +2266,13 @@ Mavlink::start(int argc, char *argv[]) } void -Mavlink::status() +Mavlink::display_status() { warnx("running"); } int -Mavlink::stream(int argc, char *argv[]) +Mavlink::stream_command(int argc, char *argv[]) { const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; @@ -2360,7 +2360,7 @@ int mavlink_main(int argc, char *argv[]) // mavlink::g_mavlink->status(); } else if (!strcmp(argv[1], "stream")) { - return Mavlink::stream(argc, argv); + return Mavlink::stream_command(argc, argv); } else { usage(); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 6777d56c3..5f4117ae5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -123,9 +123,9 @@ public: /** * Display the mavlink status. */ - void status(); + void display_status(); - static int stream(int argc, char *argv[]); + static int stream_command(int argc, char *argv[]); static int instance_count(); diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 901fa8f05..734f0903a 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -47,10 +47,10 @@ #include "mavlink_orb_subscription.h" MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : - _fd(orb_subscribe(_topic)), - _published(false), + next(nullptr), _topic(topic), - next(nullptr) + _fd(orb_subscribe(_topic)), + _published(false) { } diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 5ec30bd33..ed6776e5c 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -43,7 +43,11 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) +MavlinkStream::MavlinkStream() : + _last_sent(0), + _channel(MAVLINK_COMM_0), + _interval(1000000), + next(nullptr) { } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 09960d106..c24c172af 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -545,7 +545,6 @@ MulticopterPositionControl::task_main() hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; math::Vector<3> sp_move_rate; sp_move_rate.zero(); @@ -862,7 +861,7 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_velocity_enabled) { /* limit max tilt */ - if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) { + if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) { /* absolute horizontal thrust */ float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 62cee145e..9870ebd05 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -179,15 +179,21 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } -void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { FILE *f = fopen("/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); - unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", + hrt_absolute_time(), msg, (double)dt, + (double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1], + (double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]); fwrite(s, 1, n, f); - n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", + (double)acc[0], (double)acc[1], (double)acc[2], + (double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1], + (double)w_xy_gps_p, (double)w_xy_gps_v); fwrite(s, 1, n, f); free(s); } @@ -261,9 +267,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime t_prev = 0; - /* acceleration in NED frame */ - float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; - /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame @@ -285,7 +288,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) - hrt_abstime xy_src_time = 0; // time of last available position data bool gps_valid = false; // GPS is valid bool sonar_valid = false; // sonar is valid @@ -370,8 +372,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); + warnx("baro offs: %.2f", (double)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -475,7 +477,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; flow_prev = flow.flow_timestamp; - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7f && flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; @@ -497,7 +499,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); } } else { @@ -510,7 +512,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_q = flow.quality / 255.0f; float dist_bottom = - z_est[0] - surface_offset; - if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) { + if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) { /* distance to surface */ float flow_dist = dist_bottom / att.R[2][2]; /* check if flow if too large for accurate measurements */ @@ -558,7 +560,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* under ideal conditions, on 1m distance assume EPH = 10cm */ - eph_flow = 0.1 / w_flow; + eph_flow = 0.1f / w_flow; flow_valid = true; @@ -661,8 +663,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); } } @@ -746,10 +748,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* increase EPH/EPV on each step */ if (eph < max_eph_epv) { - eph *= 1.0 + dt; + eph *= 1.0f + dt; } if (epv < max_eph_epv) { - epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) + epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } /* use GPS if it's valid and reference position initialized */ @@ -758,11 +760,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); - /* try to estimate position during some time after position sources lost */ - if (use_gps_xy || use_flow) { - xy_src_time = t; - } - bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 7471faec7..d5a6b1ec4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -312,7 +312,7 @@ struct IOPacket { #define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) -#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) +#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) static const uint8_t crc8_tab[256] __attribute__((unused)) = { diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index d4c25911e..cd134ccb4 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -37,6 +37,7 @@ */ #include +#include #include // required for task_create #include @@ -303,14 +304,12 @@ user_start(int argc, char *argv[]) */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - struct mallinfo minfo = mallinfo(); - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)minfo.mxordblk); + (unsigned)mallinfo().mxordblk); last_debug_time = hrt_absolute_time(); } } diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index db1836f4a..50108ea1b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -119,7 +119,6 @@ uint16_t r_page_raw_rc_input[] = [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_FRAME_COUNT] = 0, [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, - [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; @@ -670,7 +669,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 16fcb4c26..8908adf4c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -649,7 +649,7 @@ Sensors::parameters_update() if (!isfinite(tmpScaleFactor) || (tmpRevFactor < 0.000001f) || (tmpRevFactor > 0.2f)) { - warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0.0f; rc_valid = false; diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 8e9c2bfcf..52ae77de5 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -63,7 +63,7 @@ struct hx_stream { /* TX state */ int fd; bool tx_error; - uint8_t *tx_buf; + const uint8_t *tx_buf; unsigned tx_resid; uint32_t tx_crc; enum { diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 092c0e2b0..4b22a46d0 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -208,7 +208,6 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl char geomname[8]; int s[4]; int used; - const char *end = buf + buflen; /* enforce that the mixer ends with space or a new line */ for (int i = buflen - 1; i >= 0; i--) { @@ -302,7 +301,6 @@ MultirotorMixer::mix(float *outputs, unsigned space) //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float min_out = 0.0f; float max_out = 0.0f; - float scale_yaw = 1.0f; /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { @@ -327,7 +325,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) } /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ - if (min_out < 0.0) { + if (min_out < 0.0f) { float scale_in = thrust / (thrust - min_out); /* mix again with adjusted controls */ diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index 695574fdc..0548a9f7d 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -133,7 +133,7 @@ int lock_otp(void) // COMPLETE, BUSY, or other flash error? -int F_GetStatus(void) +static int F_GetStatus(void) { int fs = F_COMPLETE; diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 7a499ca72..e44e6cdb0 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -96,8 +96,6 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; -static sem_t param_sem = { .semcount = 1 }; - /** lock the parameter store */ static void param_lock(void) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index ea5ba9e52..c733a53d7 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -97,7 +97,6 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ } unsigned progress; - uint16_t temp_pwm; /* then set effective_pwm based on state */ switch (limit->state) { diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index fcc9b8366..a925cdd40 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -91,7 +91,7 @@ static void mtd_test(void); static void mtd_erase(char *partition_names[], unsigned n_partitions); static void mtd_readtest(char *partition_names[], unsigned n_partitions); static void mtd_rwtest(char *partition_names[], unsigned n_partitions); -static void mtd_print_info(); +static void mtd_print_info(void); static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); @@ -104,6 +104,16 @@ static unsigned n_partitions_current = 0; static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"}; static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]); +static void +mtd_status(void) +{ + if (!attached) + errx(1, "MTD driver not started"); + + mtd_print_info(); + exit(0); +} + int mtd_main(int argc, char *argv[]) { if (argc >= 2) { @@ -355,7 +365,7 @@ static ssize_t mtd_get_partition_size(void) return partsize; } -void mtd_print_info() +void mtd_print_info(void) { if (!attached) exit(1); @@ -385,16 +395,6 @@ mtd_test(void) exit(1); } -void -mtd_status(void) -{ - if (!attached) - errx(1, "MTD driver not started"); - - mtd_print_info(); - exit(0); -} - void mtd_erase(char *partition_names[], unsigned n_partitions) { @@ -428,7 +428,7 @@ mtd_readtest(char *partition_names[], unsigned n_partitions) uint8_t v[128]; for (uint8_t i = 0; i < n_partitions; i++) { - uint32_t count = 0; + ssize_t count = 0; printf("reading %s expecting %u bytes\n", partition_names[i], expected_size); int fd = open(partition_names[i], O_RDONLY); if (fd == -1) { @@ -459,8 +459,8 @@ mtd_rwtest(char *partition_names[], unsigned n_partitions) uint8_t v[128], v2[128]; for (uint8_t i = 0; i < n_partitions; i++) { - uint32_t count = 0; - off_t offset = 0; + ssize_t count = 0; + off_t offset = 0; printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size); int fd = open(partition_names[i], O_RDWR); if (fd == -1) { diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index d92ee88ef..328205e29 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,7 +63,7 @@ static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val, bool fail_on_not_found); static void do_compare(const char* name, const char* vals[], unsigned comparisons); -static void do_reset(); +static void do_reset(void); int param_main(int argc, char *argv[]) @@ -416,7 +416,7 @@ do_compare(const char* name, const char* vals[], unsigned comparisons) } static void -do_reset() +do_reset(void) { param_reset_all(); diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index 50dece816..0ecdc8eb9 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -65,7 +65,7 @@ int test_conv(int argc, char *argv[]) float f = i/10000.0f; float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); if (fabsf(f - fres) > 0.0001f) { - warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres); + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); return 1; } } diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 96be1e8df..86f23b485 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -102,7 +102,7 @@ test_file(int argc, char *argv[]) } /* perform tests for a range of chunk sizes */ - unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; + int chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { @@ -116,7 +116,7 @@ test_file(int argc, char *argv[]) uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); /* fill write buffer with known values */ - for (int i = 0; i < sizeof(write_buf); i++) { + for (size_t i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ write_buf[i] = i+11; } @@ -224,9 +224,6 @@ test_file(int argc, char *argv[]) return 1; } - /* compare value */ - bool compare_ok = true; - for (int j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j]) { warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c index ef555f6c3..8db3ea5ae 100644 --- a/src/systemcmds/tests/test_file2.c +++ b/src/systemcmds/tests/test_file2.c @@ -49,6 +49,8 @@ #include #include +#include "tests.h" + #define FLAG_FSYNC 1 #define FLAG_LSEEK 2 @@ -85,9 +87,9 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t buffer[j] = get_value(ofs); ofs++; } - if (write(fd, buffer, sizeof(buffer)) != sizeof(buffer)) { - printf("write failed at offset %u\n", ofs); - exit(1); + if (write(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) { + printf("write failed at offset %u\n", ofs); + exit(1); } if (flags & FLAG_FSYNC) { fsync(fd); @@ -116,7 +118,7 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t printf("read ofs=%u\r", ofs); } counter++; - if (read(fd, buffer, sizeof(buffer)) != sizeof(buffer)) { + if (read(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) { printf("read failed at offset %u\n", ofs); exit(1); } diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c index 4921c9bbb..e33cc6ef7 100644 --- a/src/systemcmds/tests/test_float.c +++ b/src/systemcmds/tests/test_float.c @@ -94,7 +94,7 @@ int test_float(int argc, char *argv[]) printf("\t success: asinf(1.0f) == 1.57079f\n"); } else { - printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one); + printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", (double)asinf_one); ret = -1; } @@ -128,7 +128,7 @@ int test_float(int argc, char *argv[]) float sinf_zero_one = sinf(0.1f); - if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) { + if (fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) { printf("\t success: sinf(0.1f) == 0.09983f\n"); } else { @@ -155,7 +155,7 @@ int test_float(int argc, char *argv[]) } char sbuf[30]; - sprintf(sbuf, "%8.4f", 0.553415f); + sprintf(sbuf, "%8.4f", (double)0.553415f); if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' && sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5' @@ -166,7 +166,7 @@ int test_float(int argc, char *argv[]) ret = -5; } - sprintf(sbuf, "%8.4f", -0.553415f); + sprintf(sbuf, "%8.4f", (double)-0.553415f); if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' && sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5' @@ -205,7 +205,7 @@ int test_float(int argc, char *argv[]) printf("\t success: (float) 1.55f == 1.55 (double)\n"); } else { - printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", f1); + printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", (double)f1); ret = -8; } diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 693a208ba..00c649c75 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -52,10 +52,6 @@ using namespace math; -const char* formatResult(bool res) { - return res ? "OK" : "ERROR"; -} - int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 0b826b826..8ab8fa2d6 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -81,7 +81,7 @@ int test_mixer(int argc, char *argv[]) warnx("testing mixer"); - char *filename = "/etc/mixers/IO_pass.mix"; + const char *filename = "/etc/mixers/IO_pass.mix"; if (argc > 1) filename = argv[1]; @@ -100,8 +100,6 @@ int test_mixer(int argc, char *argv[]) * e.g. on PX4IO. */ - unsigned nused = 0; - const unsigned chunk_size = 64; MixerGroup mixer_group(mixer_callback, 0); @@ -124,7 +122,6 @@ int test_mixer(int argc, char *argv[]) return 1; /* FIRST mark the mixer as invalid */ - bool mixer_ok = false; /* THEN actually delete it */ mixer_group.reset(); char mixer_text[256]; /* large enough for one mixer */ @@ -140,7 +137,6 @@ int test_mixer(int argc, char *argv[]) /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { - bool mixer_ok = false; return 1; } @@ -156,15 +152,6 @@ int test_mixer(int argc, char *argv[]) /* if anything was parsed */ if (resid != mixer_text_length) { - - /* only set mixer ok if no residual is left over */ - if (resid == 0) { - mixer_ok = true; - } else { - /* not yet reached the end of the mixer, set as not ok */ - mixer_ok = false; - } - warnx("used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ @@ -192,7 +179,7 @@ int test_mixer(int argc, char *argv[]) should_arm = true; /* run through arming phase */ - for (int i = 0; i < output_max; i++) { + for (unsigned i = 0; i < output_max; i++) { actuator_controls[i] = 0.1f; r_page_servo_disarmed[i] = PWM_LOWEST_MIN; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; @@ -213,7 +200,7 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && @@ -228,7 +215,7 @@ int test_mixer(int argc, char *argv[]) return 1; } - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); sleepcount++; @@ -244,7 +231,7 @@ int test_mixer(int argc, char *argv[]) for (int j = -jmax; j <= jmax; j++) { - for (int i = 0; i < output_max; i++) { + for (unsigned i = 0; i < output_max; i++) { actuator_controls[i] = j/10.0f + 0.1f * i; r_page_servo_disarmed[i] = PWM_LOWEST_MIN; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; @@ -257,11 +244,11 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); warnx("mixed %d outputs (max %d)", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { - printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; } @@ -282,7 +269,7 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (r_page_servos[i] != r_page_servo_disarmed[i]) { @@ -316,7 +303,7 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { /* predict value */ servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; @@ -333,7 +320,7 @@ int test_mixer(int argc, char *argv[]) /* check post ramp phase */ if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { - printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; } diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index b9041b013..addd57bea 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -65,7 +65,6 @@ int test_ppm_loopback(int argc, char *argv[]) int _rc_sub = orb_subscribe(ORB_ID(input_rc)); int servo_fd, result; - servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; servo_position_t pos; servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR); diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 57c0e7f4c..c9f9ef439 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include "tests.h" diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index ef8512bf5..9c6951ca2 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -51,6 +51,7 @@ #include #include +#include #include -- cgit v1.2.3 From e8c34fa174c6a47d5d2def2417c58beb846bb30f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 10:25:57 +0200 Subject: force setpoint uorb topic: add yaw field --- src/modules/uORB/topics/vehicle_force_setpoint.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/modules/uORB/topics/vehicle_force_setpoint.h index cd948985f..e3a7360b2 100644 --- a/src/modules/uORB/topics/vehicle_force_setpoint.h +++ b/src/modules/uORB/topics/vehicle_force_setpoint.h @@ -52,6 +52,7 @@ struct vehicle_force_setpoint_s { float x; /**< in N NED */ float y; /**< in N NED */ float z; /**< in N NED */ + float yaw; /**< right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) */ }; /**< Desired force in NED frame */ /** -- cgit v1.2.3 From a4315aee2ee69068c9a13568091805e20d0b04c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 10:47:08 +0200 Subject: navigator: mission mode fixed --- src/modules/navigator/mission.cpp | 63 ++++++++++++++++----------------------- src/modules/navigator/mission.h | 21 ++----------- 2 files changed, 27 insertions(+), 57 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index fbfd1259d..f207776a9 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -70,9 +70,9 @@ Mission::Mission(Navigator *navigator, const char *name) : { /* load initial params */ updateParams(); + /* set initial mission items */ on_inactive(); - } Mission::~Mission() @@ -121,16 +121,12 @@ Mission::on_active() /* reset mission items if needed */ if (onboard_updated || offboard_updated) { set_mission_items(); - - _navigator->set_position_setpoint_triplet_updated(); } /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { advance_mission(); set_mission_items(); - - _navigator->set_position_setpoint_triplet_updated(); } } @@ -223,13 +219,16 @@ Mission::advance_mission() void Mission::set_mission_items() { + /* make sure param is up to date */ + updateParams(); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* set previous position setpoint to current */ set_previous_pos_setpoint(); /* try setting onboard mission item */ - if (set_mission_item(true, false, &_mission_item)) { + if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running"); @@ -237,7 +236,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (set_mission_item(false, false, &_mission_item)) { + } else if (read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); @@ -284,7 +283,8 @@ Mission::set_mission_items() /* try to read next mission item */ struct mission_item_s mission_item_next; - if (!set_mission_item(_mission_type == MISSION_TYPE_ONBOARD, true, &mission_item_next)) { + + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { /* got next mission item, update setpoint triplet */ mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); @@ -297,11 +297,8 @@ Mission::set_mission_items() } bool -Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item) +Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item) { - /* make sure param is up to date */ - updateParams(); - /* select onboard/offboard mission */ int *mission_index_ptr; struct mission_s *mission; @@ -310,12 +307,8 @@ Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *m if (onboard) { /* onboard mission */ - if (!_param_onboard_enabled.get()) { - return false; - } - mission_index_next = _current_onboard_mission_index + 1; - mission_index_ptr = next_item ? &mission_index_next : &_current_onboard_mission_index; + mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next; mission = &_onboard_mission; @@ -324,7 +317,7 @@ Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *m } else { /* offboard mission */ mission_index_next = _current_offboard_mission_index + 1; - mission_index_ptr = next_item ? &mission_index_next : &_current_offboard_mission_index; + mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next; mission = &_offboard_mission; @@ -336,27 +329,20 @@ Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *m } } - if (*mission_index_ptr >= 0 && *mission_index_ptr < (int)mission->count) { - struct mission_item_s new_mission_item; - - if (read_mission_item(dm_item, true, mission_index_ptr, &new_mission_item)) { - memcpy(&mission_item, &new_mission_item, sizeof(struct mission_item_s)); - return true; - } + if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { + /* mission item index out of bounds */ + return false; } - return false; -} -bool -Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index_ptr, - struct mission_item_s *new_mission_item) -{ /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ for (int i = 0; i < 10; i++) { const ssize_t len = sizeof(struct mission_item_s); + /* read mission item to temp storage first to not overwrite current mission item if data damaged */ + struct mission_item_s mission_item_tmp; + /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index_ptr, new_mission_item, len) != len) { + if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_log_critical(_navigator->get_mavlink_fd(), "#audio: ERROR waypoint could not be read"); @@ -364,18 +350,17 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ - if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) { /* do DO_JUMP as many times as requested */ - if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) { /* only raise the repeat count if this is for the current mission item * but not for the next mission item */ if (is_current) { - (new_mission_item->do_jump_current_count)++; + (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, - new_mission_item, len) != len) { + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), @@ -385,7 +370,8 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index_ptr = new_mission_item->do_jump_mission_index; + *mission_index_ptr = mission_item_tmp.do_jump_mission_index; + } else { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: DO JUMP repetitions completed"); @@ -395,6 +381,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } else { /* if it's not a DO_JUMP, then we were successful */ + memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s)); return true; } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 7d07860de..29e4d41f6 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -97,27 +97,10 @@ private: void set_mission_items(); /** - * Try to set given mission item from an offboard/onboard mission item - * @return true if mission item successfully set - */ - bool set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item); - - /** - * Try to set the next position setpoint from an onboard mission item - */ - void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp); - - /** - * Try to set the next position setpoint from an offboard mission item - */ - void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp); - - /** - * Read a mission item from the dataman and watch out for DO_JUMPS + * Read current or next mission item from the dataman and watch out for DO_JUMPS * @return true if successful */ - bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, - struct mission_item_s *new_mission_item); + bool read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item); /** * Report that a mission item has been reached -- cgit v1.2.3 From 59775efaad782a1e4f5e346832407c71f1355c89 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 12:02:29 +0200 Subject: navigator: takeoff on start of mission implemented --- src/modules/navigator/mission.cpp | 133 ++++++++++++++++++++++++++++++-------- src/modules/navigator/mission.h | 3 + 2 files changed, 109 insertions(+), 27 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index f207776a9..2e5047e2c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -60,13 +60,16 @@ Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _param_onboard_enabled(this, "ONBOARD_EN"), + _param_takeoff_alt(this, "TAKEOFF_ALT"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE) + _mission_type(MISSION_TYPE_NONE), + _need_takeoff(true), + _takeoff(false) { /* load initial params */ updateParams(); @@ -94,6 +97,10 @@ Mission::on_inactive() if (offboard_updated) { update_offboard_mission(); } + + if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) { + _need_takeoff = true; + } } void @@ -127,6 +134,12 @@ Mission::on_active() if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { advance_mission(); set_mission_items(); + + } else { + /* if waypoint position reached allow loiter on the setpoint */ + if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { + _navigator->set_can_loiter_at_sp(true); + } } } @@ -148,6 +161,7 @@ Mission::update_onboard_mission() } /* otherwise, just leave it */ } + } else { _onboard_mission.count = 0; _onboard_mission.current_index = 0; @@ -164,10 +178,12 @@ Mission::update_offboard_mission() if (_offboard_mission.current_index >= 0 && _offboard_mission.current_index < (int)_offboard_mission.count) { _current_offboard_mission_index = _offboard_mission.current_index; + } else { /* if less WPs available, reset to first WP */ if (_current_offboard_mission_index >= (int)_offboard_mission.count) { _current_offboard_mission_index = 0; + /* if not initialized, set it to 0 */ } else if (_current_offboard_mission_index < 0) { _current_offboard_mission_index = 0; @@ -181,6 +197,7 @@ Mission::update_offboard_mission() if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } @@ -189,11 +206,13 @@ Mission::update_offboard_mission() (size_t)_offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt); + } else { _offboard_mission.count = 0; _offboard_mission.current_index = 0; _current_offboard_mission_index = 0; } + report_current_offboard_mission_item(); } @@ -201,18 +220,23 @@ Mission::update_offboard_mission() void Mission::advance_mission() { - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - - case MISSION_TYPE_NONE: - default: - break; + if (_takeoff) { + _takeoff = false; + + } else { + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; + } } } @@ -271,26 +295,81 @@ Mission::set_mission_items() return; } - /* new current mission item set */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - _navigator->set_can_loiter_at_sp(false); - reset_mission_item_reached(); + /* do takeoff on first waypoint for rotary wing vehicles */ + if (_navigator->get_vstatus()->is_rotary_wing) { + /* force takeoff if landed (additional protection) */ + if (!_takeoff && _navigator->get_vstatus()->condition_landed) { + _need_takeoff = true; + } - if (_mission_type == MISSION_TYPE_OFFBOARD) { - report_current_offboard_mission_item(); + /* new current mission item set, check if we need takeoff */ + if (_need_takeoff && ( + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) { + _takeoff = true; + _need_takeoff = false; + } } - // TODO: report onboard mission item somehow - /* try to read next mission item */ - struct mission_item_s mission_item_next; + if (_takeoff) { + /* do takeoff before going to setpoint */ + /* set mission item as next position setpoint */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next); - if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { - /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + /* calculate takeoff altitude */ + float takeoff_alt = _mission_item.altitude; + if (_mission_item.altitude_is_relative) { + takeoff_alt += _navigator->get_home_position()->alt; + } + + if (_navigator->get_vstatus()->condition_landed) { + takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); + + } else { + takeoff_alt = _navigator->get_home_position()->alt + _param_takeoff_alt.get(); + } + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", takeoff_alt - _navigator->get_home_position()->alt); + + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; + + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); } else { - /* next mission item is not available */ - pos_sp_triplet->next.valid = false; + /* set current position setpoint from mission item */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + + /* require takeoff after landing or idle */ + if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { + _need_takeoff = true; + } + + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); + + if (_mission_type == MISSION_TYPE_OFFBOARD) { + report_current_offboard_mission_item(); + } + // TODO: report onboard mission item somehow + + /* try to read next mission item */ + struct mission_item_s mission_item_next; + + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } } _navigator->set_position_setpoint_triplet_updated(); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 29e4d41f6..d4808b6f4 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -123,12 +123,15 @@ private: void publish_mission_result(); control::BlockParamFloat _param_onboard_enabled; + control::BlockParamFloat _param_takeoff_alt; struct mission_s _onboard_mission; struct mission_s _offboard_mission; int _current_onboard_mission_index; int _current_offboard_mission_index; + bool _need_takeoff; + bool _takeoff; orb_advert_t _mission_result_pub; struct mission_result_s _mission_result; -- cgit v1.2.3 From b9e2fa2c0dc77e41846b6982f057cbbc9e60be89 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 12:03:48 +0200 Subject: mc_pos_control: compiler warning fix --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index c24c172af..bcc3e2ae7 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -466,7 +466,7 @@ MulticopterPositionControl::update_ref() { if (_local_pos.ref_timestamp != _ref_timestamp) { double lat_sp, lon_sp; - float alt_sp; + float alt_sp = 0.0f; if (_ref_timestamp != 0) { /* calculate current position setpoint in global frame */ -- cgit v1.2.3 From 8e41cbfdfa2083c421954b6d9aadde5c0e508cdc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:18:38 +0200 Subject: mavlink: Warning fixes --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- src/modules/mavlink/mavlink_main.h | 12 ++++++------ src/modules/mavlink/mavlink_stream.cpp | 4 ++-- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 43acee96f..026a4d6c9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -474,7 +474,7 @@ Mavlink::get_instance_id() return _instance_id; } -const mavlink_channel_t +mavlink_channel_t Mavlink::get_channel() { return _channel; @@ -2109,7 +2109,7 @@ Mavlink::task_main(int argc, char *argv[]) write_ptr = (uint8_t*)&msg; // Pull a single message from the buffer - int read_count = available; + size_t read_count = available; if (read_count > sizeof(mavlink_message_t)) { read_count = sizeof(mavlink_message_t); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5f4117ae5..f94036a17 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -213,15 +213,15 @@ public: */ int enable_flow_control(bool enabled); - const mavlink_channel_t get_channel(); + mavlink_channel_t get_channel(); - void configure_stream_threadsafe(const char *stream_name, const float rate); + void configure_stream_threadsafe(const char *stream_name, float rate); bool _task_should_exit; /**< if true, mavlink task should exit */ int get_mavlink_fd() { return _mavlink_fd; } - MavlinkStream * get_streams() { return _streams; } const + MavlinkStream * get_streams() const { return _streams; } /* Functions for waiting to start transmission until message received. */ @@ -311,15 +311,15 @@ private: pthread_mutex_t _message_buffer_mutex; - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _txerr_perf; /**< TX error counter */ - bool _param_initialized; param_t _param_system_id; param_t _param_component_id; param_t _param_system_type; param_t _param_use_hil_gps; + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ + /** * Send one parameter. * diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index ed6776e5c..7279206db 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -44,10 +44,10 @@ #include "mavlink_main.h" MavlinkStream::MavlinkStream() : - _last_sent(0), + next(nullptr), _channel(MAVLINK_COMM_0), _interval(1000000), - next(nullptr) + _last_sent(0) { } -- cgit v1.2.3 From 3f6851b9879c2e4d764926a3bc29ff800a17b73d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:19:08 +0200 Subject: Re-send the RC config warnings once the GCS is connected, fixed compile warnings --- .../commander/accelerometer_calibration.cpp | 3 ++- src/modules/commander/calibration_routines.cpp | 7 +++--- src/modules/commander/commander.cpp | 26 +++++++++++++++++----- 3 files changed, 27 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 24da452b1..be465ab76 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -131,6 +131,7 @@ #include #include #include +#include #include #include #include @@ -526,7 +527,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0f) { + if (fabsf(det) < FLT_EPSILON) { return ERROR; // Singular matrix } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 9d79124e7..43f341ae7 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -40,6 +40,7 @@ */ #include +#include #include "calibration_routines.h" @@ -179,9 +180,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); - aA = (aA == 0.0f) ? 1.0f : aA; - aB = (aB == 0.0f) ? 1.0f : aB; - aC = (aC == 0.0f) ? 1.0f : aC; + aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA; + aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB; + aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC; //Compute next iteration nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb42889ea..ef4c18ea6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -371,16 +371,16 @@ void print_status() static orb_advert_t status_pub; -transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy) +transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char *armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { - mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); @@ -507,7 +507,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); + mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", + (double)cmd->param1, + (double)cmd->param2, + (double)cmd->param3, + (double)cmd->param4, + (double)cmd->param5, + (double)cmd->param6, + (double)cmd->param7); } } break; @@ -752,7 +759,6 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; - hrt_abstime last_auto_state_valid = 0; bool status_changed = true; bool param_init_forced = true; @@ -862,6 +868,7 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = false; bool main_state_changed = false; bool failsafe_old = false; + bool system_checked = false; while (!thread_should_exit) { @@ -915,6 +922,15 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } + /* Perform system checks (again) once params are loaded and MAVLink is up. */ + if (!system_checked && mavlink_fd && + (telemetry.heartbeat_time > 0) && + (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { + + (void)rc_calibration_check(mavlink_fd); + system_checked = true; + } + orb_check(sp_man_sub, &updated); if (updated) { -- cgit v1.2.3 From ee8f4dcee639effe982bbb099a6f86f7c4fce5e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:19:44 +0200 Subject: systemcmds: Warning fixes --- src/systemcmds/mtd/24xxxx_mtd.c | 4 +++- src/systemcmds/pwm/pwm.c | 3 ++- src/systemcmds/tests/test_conv.cpp | 2 +- src/systemcmds/tests/test_file.c | 3 ++- src/systemcmds/tests/test_float.c | 14 +++++++------- src/systemcmds/tests/test_mtd.c | 2 ++ 6 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index a3f9dffff..72200f418 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -237,8 +237,10 @@ void at24c_test(void) } else if (result != 1) { vdbg("unexpected %u\n", result); } - if ((count % 100) == 0) + + if ((count % 100) == 0) { vdbg("test %u errors %u\n", count, errors); + } } } diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 1828c660f..e0e6ca537 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -515,7 +515,8 @@ pwm_main(int argc, char *argv[]) ret = poll(&fds, 1, 0); if (ret > 0) { - int ret = read(0, &c, 1); + ret = read(0, &c, 1); + if (ret > 0) { /* reset output to the last value */ for (unsigned i = 0; i < servo_count; i++) { diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index 0ecdc8eb9..fda949c61 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -65,7 +65,7 @@ int test_conv(int argc, char *argv[]) float f = i/10000.0f; float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); if (fabsf(f - fres) > 0.0001f) { - warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); return 1; } } diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 86f23b485..570583dee 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -149,6 +149,8 @@ test_file(int argc, char *argv[]) } end = hrt_absolute_time(); + warnx("write took %llu us", (end - start)); + close(fd); fd = open("/fs/microsd/testfile", O_RDONLY); @@ -192,7 +194,6 @@ test_file(int argc, char *argv[]) warnx("testing aligned writes - please wait.. (CTRL^C to abort)"); - start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf, chunk_sizes[c]); diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c index e33cc6ef7..bb8b6c7f5 100644 --- a/src/systemcmds/tests/test_float.c +++ b/src/systemcmds/tests/test_float.c @@ -68,7 +68,7 @@ int test_float(int argc, char *argv[]) float sinf_one = sinf(1.0f); float sqrt_two = sqrt(2.0f); - if (sinf_zero == 0.0f) { + if (fabsf(sinf_zero) < FLT_EPSILON) { printf("\t success: sinf(0.0f) == 0.0f\n"); } else { @@ -136,7 +136,7 @@ int test_float(int argc, char *argv[]) ret = -2; } - if (sqrt_two == 1.41421356f) { + if (fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON) { printf("\t success: sqrt(2.0f) == 1.41421f\n"); } else { @@ -188,7 +188,7 @@ int test_float(int argc, char *argv[]) double d1d2 = d1 * d2; - if (d1d2 == 2.022200000000000219557705349871) { + if (fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON) { printf("\t success: 1.0111 * 2.0 == 2.0222\n"); } else { @@ -201,7 +201,7 @@ int test_float(int argc, char *argv[]) // Assign value of f1 to d1 d1 = f1; - if (f1 == (float)d1) { + if (fabsf(f1 - (float)d1) < FLT_EPSILON) { printf("\t success: (float) 1.55f == 1.55 (double)\n"); } else { @@ -216,7 +216,7 @@ int test_float(int argc, char *argv[]) double sin_one = sin(1.0); double atan2_ones = atan2(1.0, 1.0); - if (sin_zero == 0.0) { + if (fabs(sin_zero - 0.0) < DBL_EPSILON) { printf("\t success: sin(0.0) == 0.0\n"); } else { @@ -224,7 +224,7 @@ int test_float(int argc, char *argv[]) ret = -9; } - if (sin_one == 0.841470984807896504875657228695) { + if (fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON) { printf("\t success: sin(1.0) == 0.84147098480\n"); } else { @@ -232,7 +232,7 @@ int test_float(int argc, char *argv[]) ret = -10; } - if (atan2_ones != 0.785398) { + if (fabs(atan2_ones - 0.785398) < DBL_EPSILON) { printf("\t success: atan2(1.0, 1.0) == 0.785398\n"); } else { diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c index cdb4362ba..43231ccad 100644 --- a/src/systemcmds/tests/test_mtd.c +++ b/src/systemcmds/tests/test_mtd.c @@ -171,6 +171,8 @@ test_mtd(int argc, char *argv[]) } end = hrt_absolute_time(); + warnx("write took %llu us", (end - start)); + close(fd); fd = open(PARAM_FILE_NAME, O_RDONLY); -- cgit v1.2.3 From 35cd487a428a9c76609faa8ae0f99afdc376b5a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:20:02 +0200 Subject: drivers: Warning fixes --- src/drivers/boards/px4fmu-v1/px4fmu_init.c | 2 +- src/drivers/boards/px4fmu-v1/px4fmu_led.c | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index 4b12b75f9..293021f8b 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -99,7 +99,7 @@ * CONFIG_ARCH_LEDS configuration switch. */ __BEGIN_DECLS -extern void led_init(); +extern void led_init(void); extern void led_on(int led); extern void led_off(int led); __END_DECLS diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c index ea91f34ad..ee53fc43d 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -54,13 +54,13 @@ * CONFIG_ARCH_LEDS configuration switch. */ __BEGIN_DECLS -extern void led_init(); +extern void led_init(void); extern void led_on(int led); extern void led_off(int led); extern void led_toggle(int led); __END_DECLS -__EXPORT void led_init() +__EXPORT void led_init(void) { /* Configure LED1-2 GPIOs for output */ -- cgit v1.2.3 From 20de2da032083187bd89914942ffc8574904c885 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:20:23 +0200 Subject: Navigator: Warning fixes --- src/modules/navigator/mission_block.cpp | 4 +++- src/modules/navigator/rtl.cpp | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 9b8d3d9c7..26a573544 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -42,6 +42,8 @@ #include #include #include +#include +#include #include #include @@ -222,7 +224,7 @@ MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet } if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER - || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius() + || (fabsf(pos_sp_triplet->current.loiter_radius - _navigator->get_loiter_radius()) > FLT_EPSILON) || pos_sp_triplet->current.loiter_direction != 1 || pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 043f773a4..597a2c0ec 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -227,7 +227,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _navigator->set_can_loiter_at_sp(true); if (autoland) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside); } else { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); -- cgit v1.2.3 From a66f88b29a93be4c3f66392994d55b4dc03d428e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:20:47 +0200 Subject: systemlib: Warning fixes --- src/modules/systemlib/rc_check.c | 1 + src/modules/systemlib/systemlib.c | 3 +++ 2 files changed, 4 insertions(+) diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index c0c1a5cb4..b2b2c1290 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -42,6 +42,7 @@ #include #include +#include #include #include #include diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 57a751e1c..9fff3eb88 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -64,6 +64,9 @@ systemreset(bool to_bootloader) *(uint32_t *)0x40002850 = 0xb007b007; } up_systemreset(); + + /* lock up here */ + while(true); } static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); -- cgit v1.2.3 From 751f8462f6aa298a59619484a13adedfdd521458 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:21:23 +0200 Subject: IO firmware: Warning / bug fixes --- src/modules/px4iofirmware/i2c.c | 2 ++ src/modules/px4iofirmware/sbus.c | 6 ++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 79b6546b3..76762c0fc 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -331,6 +331,7 @@ i2c_tx_complete(void) i2c_tx_setup(); } +#ifdef DEBUG static void i2c_dump(void) { @@ -339,3 +340,4 @@ i2c_dump(void) debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE); debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2); } +#endif diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 0e7dc621c..70ccab180 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -119,13 +119,15 @@ sbus_init(const char *device) bool sbus1_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'A', 1); + char a = 'A'; + write(sbus_fd, &a, 1); } bool sbus2_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'B', 1); + char b = 'B'; + write(sbus_fd, &b, 1); } bool -- cgit v1.2.3 From 083d605f8a9b00a1551572008e487279c39f16aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:22:06 +0200 Subject: sensors: Warning fixes --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8908adf4c..8cfe35c42 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1315,7 +1315,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) { + if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) { float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor -- cgit v1.2.3 From b5f9e95af0be6208a1eb411ed56c830d762227f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:22:26 +0200 Subject: INAV: Warning fixes --- .../position_estimator_inav/position_estimator_inav_main.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 9870ebd05..488b53453 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -477,7 +478,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; flow_prev = flow.flow_timestamp; - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7f && flow.ground_distance_m != sonar_prev) { + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7f && (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { sonar_time = t; sonar_prev = flow.ground_distance_m; corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; @@ -952,11 +953,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float updates_dt = (t - updates_counter_start) * 0.000001f; warnx( "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", - accel_updates / updates_dt, - baro_updates / updates_dt, - gps_updates / updates_dt, - attitude_updates / updates_dt, - flow_updates / updates_dt); + (double)(accel_updates / updates_dt), + (double)(baro_updates / updates_dt), + (double)(gps_updates / updates_dt), + (double)(attitude_updates / updates_dt), + (double)(flow_updates / updates_dt)); updates_counter_start = t; accel_updates = 0; baro_updates = 0; -- cgit v1.2.3 From 04efee0bc8a2b0e8e9ffae4cf2c5b28bb1ba9ba9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:23:38 +0200 Subject: nshterm: Hotfix for retries counter --- src/systemcmds/nshterm/nshterm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 7d9484d3e..fca1798e6 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -60,7 +60,7 @@ nshterm_main(int argc, char *argv[]) printf("Usage: nshterm \n"); exit(1); } - uint8_t retries = 0; + unsigned retries = 0; int fd = -1; /* try the first 30 seconds */ -- cgit v1.2.3 From 0a159e1a2490e5ec7f368d938e2b0fd365d2731e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 12:31:29 +0200 Subject: mavlink, commander: bugs fixed --- src/modules/commander/commander.cpp | 2 +- src/modules/mavlink/mavlink_main.h | 10 ++-------- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0ff1195c9..bba14ea27 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1407,7 +1407,7 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, - mission_result.mission_finished); + mission_result.finished); // TODO handle mode changes by commands if (main_state_changed) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b4184b21c..67ef8d00f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -182,7 +182,7 @@ public: int send_statustext(const char *string); int send_statustext(enum MAV_SEVERITY severity, const char *string); - MavlinkStream * get_streams() { return _streams; } const + MavlinkStream * get_streams() const { return _streams; } float get_rate_mult(); @@ -193,7 +193,7 @@ public: bool get_wait_to_transmit() { return _wait_to_transmit; } bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } - bool message_buffer_write(void *ptr, int size); + bool message_buffer_write(const void *ptr, int size); void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } @@ -322,10 +322,6 @@ private: void mavlink_update_system(); - uint8_t get_system_id(); - - uint8_t get_component_id(); - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int configure_stream(const char *stream_name, const float rate); @@ -338,8 +334,6 @@ private: int message_buffer_is_empty(); - bool message_buffer_write(const void *ptr, int size); - int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); -- cgit v1.2.3 From a1655bb8c4a7f2201887a52de60a519c029a7505 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 12:42:26 +0200 Subject: rcS: start dataman before commander --- ROMFS/px4fmu_common/init.d/rcS | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 983a6a38f..b4e90c960 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -275,6 +275,11 @@ then # Try to get an USB console nshterm /dev/ttyACM0 & + # + # Start the datamanager + # + dataman start + # # Start the Commander (needs to be this early for in-air-restarts) # @@ -402,11 +407,6 @@ then fi fi - # - # Start the datamanager - # - dataman start - # # MAVLink # -- cgit v1.2.3 From 799f568ab48ca3446b24f41290589f7f3c39ef0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 14:33:56 +0200 Subject: commander: Minor checks / improvements to power enforce patch --- src/modules/commander/commander.cpp | 4 +++- src/modules/commander/state_machine_helper.cpp | 16 +++++----------- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 507f18e24..a15712a91 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -974,8 +974,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = false; } else { status.condition_power_input_valid = true; - status.avionics_power_rail_voltage = system_power.voltage5V_v; } + + /* copy avionics voltage */ + status.avionics_power_rail_voltage = system_power.voltage5V_v; } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1997729d4..7a61d481b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -141,22 +141,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if power is not good if (!status->condition_power_input_valid) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); - } - + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); valid_transition = false; } // Fail transition if power levels on the avionics rail - // are insufficient - if ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.5f)) { - - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); - } + // are measured but are insufficient + if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.9f)) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); valid_transition = false; } } -- cgit v1.2.3 From ad4411bfc1894c8a9ca42f563f7bf8207b39e7c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 16:32:32 +0200 Subject: Fix line endings for patch --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 488b53453..5b312941e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -478,7 +478,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; flow_prev = flow.flow_timestamp; - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7f && (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { + if ((flow.ground_distance_m > 0.31f) && + (flow.ground_distance_m < 4.0f) && + (att.R[2][2] > 0.7f) && + (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { + sonar_time = t; sonar_prev = flow.ground_distance_m; corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; -- cgit v1.2.3 From bbdf2bc07aaaa363ef58f15831511c64c2d856bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 16:58:05 +0200 Subject: commander: Notify negative on manual arming fails --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e4a709ef6..efa26eb97 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1342,6 +1342,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + tune_negative(true); } /* evaluate the main state machine according to mode switches */ -- cgit v1.2.3 From 8408993a676acd0149e880ffeded74ce9c67b9c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 17:45:50 +0200 Subject: Hotfix, non-code change: Raise RC config error warnings to audio --- src/modules/systemlib/rc_check.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index b2b2c1290..b35b333af 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -99,32 +99,32 @@ int rc_calibration_check(int mavlink_fd) { /* assert min..center..max ordering */ if (param_min < 500) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); + mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MIN < 500", i+1); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_max > 2500) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); + mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MAX > 2500", i+1); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim < param_min) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min); + mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim > param_max) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max); + mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max); /* give system time to flush error message in case there are more */ usleep(100000); } /* assert deadzone is sane */ if (param_dz > 500) { - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); + mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_DZ > 500", i+1); /* give system time to flush error message in case there are more */ usleep(100000); count++; @@ -140,7 +140,7 @@ int rc_calibration_check(int mavlink_fd) { /* sanity checks pass, enable channel */ if (count) { - mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + mavlink_log_critical(mavlink_fd, "#audio ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); warnx("ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); usleep(100000); } -- cgit v1.2.3 From 45433c3711dba5c972e02e8c9263853164670a86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:52:18 +0200 Subject: Hotfix: Fix use of circuit breaker param --- src/modules/systemlib/circuit_breaker.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index b59531d69..8f697749e 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -86,7 +86,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; - (void)param_get(param_find(breaker), val); + (void)param_get(param_find(breaker), &val); return (val == magic); } -- cgit v1.2.3 From fb98251e4d08d351c7ba404e3816958435c57515 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:52:36 +0200 Subject: Warning fixes in commander --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 244513375..423ce2f23 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -182,7 +182,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && (status->avionics_power_rail_voltage < 4.9f)) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); valid_transition = false; } } @@ -638,7 +638,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { - int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); -- cgit v1.2.3 From 534c377ec6d09aff0feed9eb7e70c9188abcda8c Mon Sep 17 00:00:00 2001 From: philipoe Date: Tue, 1 Jul 2014 10:49:59 +0200 Subject: TECS: Small fix to throttle feed-forward --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 3730b1920..6386e37a0 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -310,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f); if (STEdot_dem >= 0) { - ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr); + ff_throttle = nomThr + STEdot_dem / _STEdot_max * (_THRmaxf - nomThr); } else { ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; -- cgit v1.2.3 From 15a7a0ce7fd01717e597a470b732d623c5fda5f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 11:31:24 +0200 Subject: autoconfig: Enforce clean defaults --- ROMFS/px4fmu_common/init.d/rcS | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f70e0ed77..9a77da9f9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -173,6 +173,10 @@ then # if [ $DO_AUTOCONFIG == yes ] then + # We can't be sure the defaults haven't changed, so + # if someone requests a re-configuration, we do it + # cleanly from scratch + param reset param set SYS_AUTOCONFIG 0 param save fi -- cgit v1.2.3 From 6042999aab26276ff87e7274b97b69200919b86f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 11:59:25 +0200 Subject: Add vital comment to circuit breaker --- src/modules/systemlib/circuit_breaker.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 86439e2a5..1175dbce8 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -40,6 +40,15 @@ #ifndef CIRCUIT_BREAKER_H_ #define CIRCUIT_BREAKER_H_ +/* SAFETY WARNING -- SAFETY WARNING -- SAFETY WARNING + * + * OBEY THE DOCUMENTATION FOR ALL CIRCUIT BREAKERS HERE, + * ENSURE TO READ CAREFULLY ALL SAFETY WARNINGS. + * http://pixhawk.org/dev/circuit_breakers + * + * CIRCUIT BREAKERS ARE NOT PART OF THE STANDARD OPERATION PROCEDURE + * AND MAY DISABLE CHECKS THAT ARE VITAL FOR SAFE FLIGHT. + */ #define CBRK_SUPPLY_CHK_KEY 894281 #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 -- cgit v1.2.3 From 29cd95ebad11380026c58343deeaf5bf334ad01c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 12:05:10 +0200 Subject: Move param reset to right autoconfig check --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 9a77da9f9..5856ba84f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -132,6 +132,10 @@ then # if param compare SYS_AUTOCONFIG 1 then + # We can't be sure the defaults haven't changed, so + # if someone requests a re-configuration, we do it + # cleanly from scratch + param reset set DO_AUTOCONFIG yes else set DO_AUTOCONFIG no @@ -173,10 +177,6 @@ then # if [ $DO_AUTOCONFIG == yes ] then - # We can't be sure the defaults haven't changed, so - # if someone requests a re-configuration, we do it - # cleanly from scratch - param reset param set SYS_AUTOCONFIG 0 param save fi -- cgit v1.2.3 From d0b78aa8a09cd4259258ab10b77c98e907c3b460 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 13:42:32 +0200 Subject: Add param command which does not reset the autostart params to not hamper auto-config --- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- src/systemcmds/param/param.c | 28 ++++++++++++++++++++++++++++ 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5856ba84f..1c9ded6a8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -134,8 +134,8 @@ then then # We can't be sure the defaults haven't changed, so # if someone requests a re-configuration, we do it - # cleanly from scratch - param reset + # cleanly from scratch (except autostart / autoconfig) + param reset_nostart set DO_AUTOCONFIG yes else set DO_AUTOCONFIG no diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 328205e29..f8bff2f6f 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -64,6 +64,7 @@ static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val, bool fail_on_not_found); static void do_compare(const char* name, const char* vals[], unsigned comparisons); static void do_reset(void); +static void do_reset_nostart(void); int param_main(int argc, char *argv[]) @@ -142,6 +143,10 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "reset")) { do_reset(); } + + if (!strcmp(argv[1], "reset_nostart")) { + do_reset_nostart(); + } } errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); @@ -427,3 +432,26 @@ do_reset(void) exit(0); } } + +static void +do_reset_nostart(void) +{ + + int32_t autostart; + int32_t autoconfig; + + (void)param_get(param_find("SYS_AUTOSTART"), &autostart); + (void)param_get(param_find("SYS_AUTOCONFIG"), &autoconfig); + + param_reset_all(); + + (void)param_set(param_find("SYS_AUTOSTART"), &autostart); + (void)param_set(param_find("SYS_AUTOCONFIG"), &autoconfig); + + if (param_save_default()) { + warnx("Param export failed."); + exit(1); + } else { + exit(0); + } +} -- cgit v1.2.3 From 8c638d2addad0831b71428591cd76629c436ee05 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 14:28:48 +0200 Subject: XML tool --- Tools/px_generate_xml.sh | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 Tools/px_generate_xml.sh diff --git a/Tools/px_generate_xml.sh b/Tools/px_generate_xml.sh new file mode 100644 index 000000000..65f0c95da --- /dev/null +++ b/Tools/px_generate_xml.sh @@ -0,0 +1,2 @@ +#!/bin/sh +python px_process_params.py --xml -- cgit v1.2.3 From 4cd66a3242aa2dfb03a3e58ffcc8d27e1350b7ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 2 Jul 2014 00:25:19 +0200 Subject: Fix sdlog2 GPS time copy for log_on_start situation --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9b071ff99..f99aec34e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1070,7 +1070,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (log_on_start) { /* check GPS topic to get GPS time */ if (log_name_timestamp) { - if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { + if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { gps_time = buf_gps_pos.time_gps_usec; } } -- cgit v1.2.3 From 756a2bb7e69113926551ee08aef0f301acaade1e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 2 Jul 2014 09:35:50 +0200 Subject: hk micro pcb startup script --- ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb new file mode 100644 index 000000000..42ec150c1 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -0,0 +1,27 @@ +#!nsh +# +# Hobbyking Micro Integrated PCB Quadcopter +# +# Anton Babushkin +# + +sh /etc/init.d/4001_quad_x + +if [ $DO_AUTOCONFIG == yes ] +then + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 +fi + +set PWM_MIN 1100 +set PWM_DISARMED 900 -- cgit v1.2.3 From 0d7ada14bb313a86cb49fb34888740486b47222d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 2 Jul 2014 09:36:47 +0200 Subject: rc.autostart entry for hk micro pcb quad --- ROMFS/px4fmu_common/init.d/rc.autostart | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index b365bd642..5d9e9502c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -136,6 +136,11 @@ then sh /etc/init.d/4011_dji_f450 fi +if param compare SYS_AUTOSTART 4020 +then + sh /etc/init.d/4020_hk_micro_pcb +fi + # # Quad + # -- cgit v1.2.3 From f428ebb04f0610c31639d8fe6d121f632c1cad1b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 2 Jul 2014 11:39:56 +0200 Subject: gps: Comment-only fix --- src/drivers/gps/gps.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 1d8ef1b99..241e3bdf3 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -109,7 +109,7 @@ private: int _serial_fd; ///< serial interface to GPS unsigned _baudrate; ///< current baudrate char _port[20]; ///< device / serial port path - volatile int _task; //< worker task + volatile int _task; ///< worker task bool _healthy; ///< flag to signal if the GPS is ok bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed bool _mode_changed; ///< flag that the GPS mode has changed -- cgit v1.2.3 From 44b875428e2137da1cb8ceb1db446ddefc12b6bf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 2 Jul 2014 14:15:04 +0200 Subject: delete mavlink header files --- .../mavlink/v1.0/ardupilotmega/ardupilotmega.h | 129 - .../include/mavlink/v1.0/ardupilotmega/mavlink.h | 27 - .../mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h | 353 -- .../mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h | 329 -- .../ardupilotmega/mavlink_msg_airspeed_autocal.h | 473 -- .../v1.0/ardupilotmega/mavlink_msg_ap_adc.h | 329 -- .../ardupilotmega/mavlink_msg_compassmot_status.h | 329 -- .../v1.0/ardupilotmega/mavlink_msg_data16.h | 249 - .../v1.0/ardupilotmega/mavlink_msg_data32.h | 249 - .../v1.0/ardupilotmega/mavlink_msg_data64.h | 249 - .../v1.0/ardupilotmega/mavlink_msg_data96.h | 249 - .../ardupilotmega/mavlink_msg_digicam_configure.h | 449 -- .../ardupilotmega/mavlink_msg_digicam_control.h | 425 -- .../ardupilotmega/mavlink_msg_fence_fetch_point.h | 257 - .../v1.0/ardupilotmega/mavlink_msg_fence_point.h | 329 -- .../v1.0/ardupilotmega/mavlink_msg_fence_status.h | 281 - .../v1.0/ardupilotmega/mavlink_msg_hwstatus.h | 233 - .../v1.0/ardupilotmega/mavlink_msg_limits_status.h | 401 -- .../v1.0/ardupilotmega/mavlink_msg_meminfo.h | 233 - .../ardupilotmega/mavlink_msg_mount_configure.h | 329 -- .../v1.0/ardupilotmega/mavlink_msg_mount_control.h | 329 -- .../v1.0/ardupilotmega/mavlink_msg_mount_status.h | 305 - .../mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h | 353 -- .../ardupilotmega/mavlink_msg_rally_fetch_point.h | 257 - .../v1.0/ardupilotmega/mavlink_msg_rally_point.h | 425 -- .../v1.0/ardupilotmega/mavlink_msg_rangefinder.h | 233 - .../ardupilotmega/mavlink_msg_sensor_offsets.h | 473 -- .../ardupilotmega/mavlink_msg_set_mag_offsets.h | 305 - .../v1.0/ardupilotmega/mavlink_msg_simstate.h | 449 -- .../mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h | 257 - .../include/mavlink/v1.0/ardupilotmega/testsuite.h | 1542 ----- .../include/mavlink/v1.0/ardupilotmega/version.h | 12 - mavlink/include/mavlink/v1.0/autoquad/autoquad.h | 132 - mavlink/include/mavlink/v1.0/autoquad/mavlink.h | 27 - .../v1.0/autoquad/mavlink_msg_aq_telemetry_f.h | 689 --- mavlink/include/mavlink/v1.0/autoquad/testsuite.h | 118 - mavlink/include/mavlink/v1.0/autoquad/version.h | 12 - mavlink/include/mavlink/v1.0/checksum.h | 91 - mavlink/include/mavlink/v1.0/common/common.h | 670 --- mavlink/include/mavlink/v1.0/common/mavlink.h | 27 - .../mavlink/v1.0/common/mavlink_msg_attitude.h | 353 -- .../v1.0/common/mavlink_msg_attitude_quaternion.h | 377 -- .../mavlink/v1.0/common/mavlink_msg_auth_key.h | 209 - .../v1.0/common/mavlink_msg_battery_status.h | 449 -- .../common/mavlink_msg_change_operator_control.h | 273 - .../mavlink_msg_change_operator_control_ack.h | 257 - .../mavlink/v1.0/common/mavlink_msg_command_ack.h | 233 - .../mavlink/v1.0/common/mavlink_msg_command_long.h | 449 -- .../mavlink/v1.0/common/mavlink_msg_data_stream.h | 257 - .../mavlink_msg_data_transmission_handshake.h | 353 -- .../mavlink/v1.0/common/mavlink_msg_debug.h | 257 - .../mavlink/v1.0/common/mavlink_msg_debug_vect.h | 297 - .../v1.0/common/mavlink_msg_distance_sensor.h | 377 -- .../v1.0/common/mavlink_msg_encapsulated_data.h | 225 - .../common/mavlink_msg_file_transfer_dir_list.h | 249 - .../v1.0/common/mavlink_msg_file_transfer_res.h | 233 - .../v1.0/common/mavlink_msg_file_transfer_start.h | 297 - .../v1.0/common/mavlink_msg_global_position_int.h | 401 -- .../mavlink_msg_global_position_setpoint_int.h | 305 - .../mavlink_msg_global_vision_position_estimate.h | 353 -- .../mavlink/v1.0/common/mavlink_msg_gps2_raw.h | 473 -- .../v1.0/common/mavlink_msg_gps_global_origin.h | 257 - .../v1.0/common/mavlink_msg_gps_inject_data.h | 273 - .../mavlink/v1.0/common/mavlink_msg_gps_raw_int.h | 425 -- .../mavlink/v1.0/common/mavlink_msg_gps_status.h | 325 -- .../mavlink/v1.0/common/mavlink_msg_heartbeat.h | 326 -- .../mavlink/v1.0/common/mavlink_msg_highres_imu.h | 545 -- .../mavlink/v1.0/common/mavlink_msg_hil_controls.h | 449 -- .../mavlink/v1.0/common/mavlink_msg_hil_gps.h | 497 -- .../v1.0/common/mavlink_msg_hil_optical_flow.h | 377 -- .../v1.0/common/mavlink_msg_hil_rc_inputs_raw.h | 521 -- .../mavlink/v1.0/common/mavlink_msg_hil_sensor.h | 545 -- .../mavlink/v1.0/common/mavlink_msg_hil_state.h | 569 -- .../v1.0/common/mavlink_msg_hil_state_quaternion.h | 561 -- .../v1.0/common/mavlink_msg_local_position_ned.h | 353 -- ...k_msg_local_position_ned_system_global_offset.h | 353 -- .../common/mavlink_msg_local_position_setpoint.h | 305 - .../mavlink/v1.0/common/mavlink_msg_log_data.h | 273 - .../mavlink/v1.0/common/mavlink_msg_log_entry.h | 305 - .../mavlink/v1.0/common/mavlink_msg_log_erase.h | 233 - .../v1.0/common/mavlink_msg_log_request_data.h | 305 - .../v1.0/common/mavlink_msg_log_request_end.h | 233 - .../v1.0/common/mavlink_msg_log_request_list.h | 281 - .../v1.0/common/mavlink_msg_manual_control.h | 329 -- .../v1.0/common/mavlink_msg_manual_setpoint.h | 353 -- .../mavlink/v1.0/common/mavlink_msg_memory_vect.h | 273 - .../mavlink/v1.0/common/mavlink_msg_mission_ack.h | 257 - .../v1.0/common/mavlink_msg_mission_clear_all.h | 233 - .../v1.0/common/mavlink_msg_mission_count.h | 257 - .../v1.0/common/mavlink_msg_mission_current.h | 209 - .../mavlink/v1.0/common/mavlink_msg_mission_item.h | 521 -- .../v1.0/common/mavlink_msg_mission_item_reached.h | 209 - .../v1.0/common/mavlink_msg_mission_request.h | 257 - .../v1.0/common/mavlink_msg_mission_request_list.h | 233 - .../mavlink_msg_mission_request_partial_list.h | 281 - .../v1.0/common/mavlink_msg_mission_set_current.h | 257 - .../mavlink_msg_mission_write_partial_list.h | 281 - .../v1.0/common/mavlink_msg_named_value_float.h | 249 - .../v1.0/common/mavlink_msg_named_value_int.h | 249 - .../common/mavlink_msg_nav_controller_output.h | 377 -- .../v1.0/common/mavlink_msg_omnidirectional_flow.h | 322 -- .../mavlink/v1.0/common/mavlink_msg_optical_flow.h | 377 -- .../v1.0/common/mavlink_msg_param_request_list.h | 233 - .../v1.0/common/mavlink_msg_param_request_read.h | 273 - .../mavlink/v1.0/common/mavlink_msg_param_set.h | 297 - .../mavlink/v1.0/common/mavlink_msg_param_value.h | 297 - .../include/mavlink/v1.0/common/mavlink_msg_ping.h | 281 - .../mavlink/v1.0/common/mavlink_msg_power_status.h | 257 - .../mavlink/v1.0/common/mavlink_msg_radio_status.h | 353 -- .../mavlink/v1.0/common/mavlink_msg_raw_imu.h | 425 -- .../mavlink/v1.0/common/mavlink_msg_raw_pressure.h | 305 - .../mavlink/v1.0/common/mavlink_msg_rc_channels.h | 689 --- .../v1.0/common/mavlink_msg_rc_channels_override.h | 425 -- .../v1.0/common/mavlink_msg_rc_channels_raw.h | 449 -- .../v1.0/common/mavlink_msg_rc_channels_scaled.h | 449 -- .../v1.0/common/mavlink_msg_request_data_stream.h | 305 - ...link_msg_roll_pitch_yaw_rates_thrust_setpoint.h | 305 - ...link_msg_roll_pitch_yaw_speed_thrust_setpoint.h | 305 - .../mavlink_msg_roll_pitch_yaw_thrust_setpoint.h | 305 - .../v1.0/common/mavlink_msg_safety_allowed_area.h | 353 -- .../common/mavlink_msg_safety_set_allowed_area.h | 401 -- .../mavlink/v1.0/common/mavlink_msg_scaled_imu.h | 425 -- .../mavlink/v1.0/common/mavlink_msg_scaled_imu2.h | 425 -- .../v1.0/common/mavlink_msg_scaled_pressure.h | 281 - .../v1.0/common/mavlink_msg_serial_control.h | 321 -- .../v1.0/common/mavlink_msg_servo_output_raw.h | 425 -- .../mavlink_msg_set_global_position_setpoint_int.h | 305 - .../common/mavlink_msg_set_gps_global_origin.h | 281 - .../mavlink_msg_set_local_position_setpoint.h | 353 -- .../mavlink/v1.0/common/mavlink_msg_set_mode.h | 257 - .../common/mavlink_msg_set_quad_motors_setpoint.h | 305 - ..._msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h | 399 -- ...link_msg_set_quad_swarm_roll_pitch_yaw_thrust.h | 324 -- .../mavlink_msg_set_roll_pitch_yaw_speed_thrust.h | 329 -- .../common/mavlink_msg_set_roll_pitch_yaw_thrust.h | 329 -- .../v1.0/common/mavlink_msg_setpoint_6dof.h | 353 -- .../v1.0/common/mavlink_msg_setpoint_8dof.h | 401 -- .../mavlink/v1.0/common/mavlink_msg_sim_state.h | 689 --- .../v1.0/common/mavlink_msg_state_correction.h | 401 -- .../mavlink/v1.0/common/mavlink_msg_statustext.h | 225 - .../mavlink/v1.0/common/mavlink_msg_sys_status.h | 497 -- .../mavlink/v1.0/common/mavlink_msg_system_time.h | 233 - .../mavlink/v1.0/common/mavlink_msg_vfr_hud.h | 329 -- .../common/mavlink_msg_vicon_position_estimate.h | 353 -- .../common/mavlink_msg_vision_position_estimate.h | 353 -- .../common/mavlink_msg_vision_speed_estimate.h | 281 - mavlink/include/mavlink/v1.0/common/testsuite.h | 5872 -------------------- mavlink/include/mavlink/v1.0/common/version.h | 12 - .../include/mavlink/v1.0/matrixpilot/matrixpilot.h | 142 - mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h | 27 - .../v1.0/matrixpilot/mavlink_msg_airspeeds.h | 353 -- .../v1.0/matrixpilot/mavlink_msg_altitudes.h | 353 -- .../mavlink_msg_flexifunction_buffer_function.h | 345 -- ...mavlink_msg_flexifunction_buffer_function_ack.h | 281 - .../mavlink_msg_flexifunction_command.h | 257 - .../mavlink_msg_flexifunction_command_ack.h | 233 - .../mavlink_msg_flexifunction_directory.h | 321 -- .../mavlink_msg_flexifunction_directory_ack.h | 329 -- .../mavlink_msg_flexifunction_read_req.h | 281 - .../matrixpilot/mavlink_msg_flexifunction_set.h | 233 - .../matrixpilot/mavlink_msg_serial_udb_extra_f13.h | 281 - .../matrixpilot/mavlink_msg_serial_udb_extra_f14.h | 449 -- .../matrixpilot/mavlink_msg_serial_udb_extra_f15.h | 234 - .../matrixpilot/mavlink_msg_serial_udb_extra_f16.h | 234 - .../mavlink_msg_serial_udb_extra_f2_a.h | 857 --- .../mavlink_msg_serial_udb_extra_f2_b.h | 977 ---- .../matrixpilot/mavlink_msg_serial_udb_extra_f4.h | 425 -- .../matrixpilot/mavlink_msg_serial_udb_extra_f5.h | 329 -- .../matrixpilot/mavlink_msg_serial_udb_extra_f6.h | 305 - .../matrixpilot/mavlink_msg_serial_udb_extra_f7.h | 329 -- .../matrixpilot/mavlink_msg_serial_udb_extra_f8.h | 353 -- .../include/mavlink/v1.0/matrixpilot/testsuite.h | 1240 ----- mavlink/include/mavlink/v1.0/matrixpilot/version.h | 12 - mavlink/include/mavlink/v1.0/mavlink_conversions.h | 135 - mavlink/include/mavlink/v1.0/mavlink_helpers.h | 574 -- .../mavlink/v1.0/mavlink_protobuf_manager.hpp | 377 -- mavlink/include/mavlink/v1.0/mavlink_types.h | 161 - mavlink/include/mavlink/v1.0/pixhawk/mavlink.h | 27 - .../v1.0/pixhawk/mavlink_msg_attitude_control.h | 401 -- .../v1.0/pixhawk/mavlink_msg_brief_feature.h | 369 -- .../v1.0/pixhawk/mavlink_msg_image_available.h | 737 --- .../pixhawk/mavlink_msg_image_trigger_control.h | 209 - .../v1.0/pixhawk/mavlink_msg_image_triggered.h | 473 -- .../mavlink/v1.0/pixhawk/mavlink_msg_marker.h | 353 -- .../v1.0/pixhawk/mavlink_msg_pattern_detected.h | 273 - .../v1.0/pixhawk/mavlink_msg_point_of_interest.h | 369 -- .../mavlink_msg_point_of_interest_connection.h | 441 -- .../mavlink_msg_position_control_setpoint.h | 305 - .../mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h | 353 -- .../v1.0/pixhawk/mavlink_msg_set_cam_shutter.h | 329 -- .../mavlink_msg_set_position_control_offset.h | 329 -- .../v1.0/pixhawk/mavlink_msg_watchdog_command.h | 281 - .../v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h | 233 - .../pixhawk/mavlink_msg_watchdog_process_info.h | 298 - .../pixhawk/mavlink_msg_watchdog_process_status.h | 329 -- mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h | 136 - mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h | 3663 ------------ mavlink/include/mavlink/v1.0/pixhawk/testsuite.h | 996 ---- mavlink/include/mavlink/v1.0/pixhawk/version.h | 12 - mavlink/include/mavlink/v1.0/protocol.h | 328 -- mavlink/include/mavlink/v1.0/sensesoar/mavlink.h | 27 - .../v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h | 253 - .../v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h | 253 - .../v1.0/sensesoar/mavlink_msg_filt_rot_vel.h | 219 - .../mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h | 254 - .../v1.0/sensesoar/mavlink_msg_obs_air_temp.h | 219 - .../v1.0/sensesoar/mavlink_msg_obs_air_velocity.h | 287 - .../v1.0/sensesoar/mavlink_msg_obs_attitude.h | 219 - .../mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h | 254 - .../v1.0/sensesoar/mavlink_msg_obs_position.h | 287 - .../mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h | 219 - .../v1.0/sensesoar/mavlink_msg_obs_velocity.h | 219 - .../mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h | 219 - .../mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h | 279 - .../mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h | 321 -- mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h | 77 - mavlink/include/mavlink/v1.0/sensesoar/testsuite.h | 676 --- mavlink/include/mavlink/v1.0/sensesoar/version.h | 12 - 218 files changed, 80550 deletions(-) delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/version.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/autoquad.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/mavlink.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/autoquad/version.h delete mode 100644 mavlink/include/mavlink/v1.0/checksum.h delete mode 100644 mavlink/include/mavlink/v1.0/common/common.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h delete mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h delete mode 100644 mavlink/include/mavlink/v1.0/common/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/common/version.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/matrixpilot/version.h delete mode 100644 mavlink/include/mavlink/v1.0/mavlink_conversions.h delete mode 100644 mavlink/include/mavlink/v1.0/mavlink_helpers.h delete mode 100644 mavlink/include/mavlink/v1.0/mavlink_protobuf_manager.hpp delete mode 100644 mavlink/include/mavlink/v1.0/mavlink_types.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/pixhawk/version.h delete mode 100644 mavlink/include/mavlink/v1.0/protocol.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/testsuite.h delete mode 100644 mavlink/include/mavlink/v1.0/sensesoar/version.h diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h deleted file mode 100644 index c887dc68a..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ /dev/null @@ -1,129 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from ardupilotmega.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef ARDUPILOTMEGA_H -#define ARDUPILOTMEGA_H - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 20, 24, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS, MAVLINK_MESSAGE_INFO_AHRS2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_ARDUPILOTMEGA - -// ENUM DEFINITIONS - - -/** @brief */ -#ifndef HAVE_ENUM_LIMITS_STATE -#define HAVE_ENUM_LIMITS_STATE -enum LIMITS_STATE -{ - LIMITS_INIT=0, /* pre-initialization | */ - LIMITS_DISABLED=1, /* disabled | */ - LIMITS_ENABLED=2, /* checking limits | */ - LIMITS_TRIGGERED=3, /* a limit has been breached | */ - LIMITS_RECOVERING=4, /* taking action eg. RTL | */ - LIMITS_RECOVERED=5, /* we're no longer in breach of a limit | */ - LIMITS_STATE_ENUM_END=6, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_LIMIT_MODULE -#define HAVE_ENUM_LIMIT_MODULE -enum LIMIT_MODULE -{ - LIMIT_GPSLOCK=1, /* pre-initialization | */ - LIMIT_GEOFENCE=2, /* disabled | */ - LIMIT_ALTITUDE=4, /* checking limits | */ - LIMIT_MODULE_ENUM_END=5, /* | */ -}; -#endif - -/** @brief Flags in RALLY_POINT message */ -#ifndef HAVE_ENUM_RALLY_FLAGS -#define HAVE_ENUM_RALLY_FLAGS -enum RALLY_FLAGS -{ - FAVORABLE_WIND=1, /* Flag set when requiring favorable winds for landing. | */ - LAND_IMMEDIATELY=2, /* Flag set when plane is to immediately descend to break altitude and land without GCS intervention. Flag not set when plane is to loiter at Rally point until commanded to land. | */ - RALLY_FLAGS_ENUM_END=3, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_PARACHUTE_ACTION -#define HAVE_ENUM_PARACHUTE_ACTION -enum PARACHUTE_ACTION -{ - PARACHUTE_DISABLE=0, /* Disable parachute release | */ - PARACHUTE_ENABLE=1, /* Enable parachute release | */ - PARACHUTE_RELEASE=2, /* Release parachute | */ - PARACHUTE_ACTION_ENUM_END=3, /* | */ -}; -#endif - -#include "../common/common.h" - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_sensor_offsets.h" -#include "./mavlink_msg_set_mag_offsets.h" -#include "./mavlink_msg_meminfo.h" -#include "./mavlink_msg_ap_adc.h" -#include "./mavlink_msg_digicam_configure.h" -#include "./mavlink_msg_digicam_control.h" -#include "./mavlink_msg_mount_configure.h" -#include "./mavlink_msg_mount_control.h" -#include "./mavlink_msg_mount_status.h" -#include "./mavlink_msg_fence_point.h" -#include "./mavlink_msg_fence_fetch_point.h" -#include "./mavlink_msg_fence_status.h" -#include "./mavlink_msg_ahrs.h" -#include "./mavlink_msg_simstate.h" -#include "./mavlink_msg_hwstatus.h" -#include "./mavlink_msg_radio.h" -#include "./mavlink_msg_limits_status.h" -#include "./mavlink_msg_wind.h" -#include "./mavlink_msg_data16.h" -#include "./mavlink_msg_data32.h" -#include "./mavlink_msg_data64.h" -#include "./mavlink_msg_data96.h" -#include "./mavlink_msg_rangefinder.h" -#include "./mavlink_msg_airspeed_autocal.h" -#include "./mavlink_msg_rally_point.h" -#include "./mavlink_msg_rally_fetch_point.h" -#include "./mavlink_msg_compassmot_status.h" -#include "./mavlink_msg_ahrs2.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // ARDUPILOTMEGA_H diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink.h deleted file mode 100644 index 551938f04..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink.h +++ /dev/null @@ -1,27 +0,0 @@ -/** @file - * @brief MAVLink comm protocol built from ardupilotmega.xml - * @see http://pixhawk.ethz.ch/software/mavlink - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#ifndef MAVLINK_STX -#define MAVLINK_STX 254 -#endif - -#ifndef MAVLINK_ENDIAN -#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN -#endif - -#ifndef MAVLINK_ALIGNED_FIELDS -#define MAVLINK_ALIGNED_FIELDS 1 -#endif - -#ifndef MAVLINK_CRC_EXTRA -#define MAVLINK_CRC_EXTRA 1 -#endif - -#include "version.h" -#include "ardupilotmega.h" - -#endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h deleted file mode 100644 index 50ddc79e5..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE AHRS PACKING - -#define MAVLINK_MSG_ID_AHRS 163 - -typedef struct __mavlink_ahrs_t -{ - float omegaIx; ///< X gyro drift estimate rad/s - float omegaIy; ///< Y gyro drift estimate rad/s - float omegaIz; ///< Z gyro drift estimate rad/s - float accel_weight; ///< average accel_weight - float renorm_val; ///< average renormalisation value - float error_rp; ///< average error_roll_pitch value - float error_yaw; ///< average error_yaw value -} mavlink_ahrs_t; - -#define MAVLINK_MSG_ID_AHRS_LEN 28 -#define MAVLINK_MSG_ID_163_LEN 28 - -#define MAVLINK_MSG_ID_AHRS_CRC 127 -#define MAVLINK_MSG_ID_163_CRC 127 - - - -#define MAVLINK_MESSAGE_INFO_AHRS { \ - "AHRS", \ - 7, \ - { { "omegaIx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs_t, omegaIx) }, \ - { "omegaIy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs_t, omegaIy) }, \ - { "omegaIz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs_t, omegaIz) }, \ - { "accel_weight", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs_t, accel_weight) }, \ - { "renorm_val", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ahrs_t, renorm_val) }, \ - { "error_rp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ahrs_t, error_rp) }, \ - { "error_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs_t, error_yaw) }, \ - } \ -} - - -/** - * @brief Pack a ahrs message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param omegaIx X gyro drift estimate rad/s - * @param omegaIy Y gyro drift estimate rad/s - * @param omegaIz Z gyro drift estimate rad/s - * @param accel_weight average accel_weight - * @param renorm_val average renormalisation value - * @param error_rp average error_roll_pitch value - * @param error_yaw average error_yaw value - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AHRS_LEN]; - _mav_put_float(buf, 0, omegaIx); - _mav_put_float(buf, 4, omegaIy); - _mav_put_float(buf, 8, omegaIz); - _mav_put_float(buf, 12, accel_weight); - _mav_put_float(buf, 16, renorm_val); - _mav_put_float(buf, 20, error_rp); - _mav_put_float(buf, 24, error_yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN); -#else - mavlink_ahrs_t packet; - packet.omegaIx = omegaIx; - packet.omegaIy = omegaIy; - packet.omegaIz = omegaIz; - packet.accel_weight = accel_weight; - packet.renorm_val = renorm_val; - packet.error_rp = error_rp; - packet.error_yaw = error_yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AHRS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS_LEN); -#endif -} - -/** - * @brief Pack a ahrs message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param omegaIx X gyro drift estimate rad/s - * @param omegaIy Y gyro drift estimate rad/s - * @param omegaIz Z gyro drift estimate rad/s - * @param accel_weight average accel_weight - * @param renorm_val average renormalisation value - * @param error_rp average error_roll_pitch value - * @param error_yaw average error_yaw value - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float omegaIx,float omegaIy,float omegaIz,float accel_weight,float renorm_val,float error_rp,float error_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AHRS_LEN]; - _mav_put_float(buf, 0, omegaIx); - _mav_put_float(buf, 4, omegaIy); - _mav_put_float(buf, 8, omegaIz); - _mav_put_float(buf, 12, accel_weight); - _mav_put_float(buf, 16, renorm_val); - _mav_put_float(buf, 20, error_rp); - _mav_put_float(buf, 24, error_yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN); -#else - mavlink_ahrs_t packet; - packet.omegaIx = omegaIx; - packet.omegaIy = omegaIy; - packet.omegaIz = omegaIz; - packet.accel_weight = accel_weight; - packet.renorm_val = renorm_val; - packet.error_rp = error_rp; - packet.error_yaw = error_yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AHRS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS_LEN); -#endif -} - -/** - * @brief Encode a ahrs struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param ahrs C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs) -{ - return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw); -} - -/** - * @brief Encode a ahrs struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param ahrs C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ahrs_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs) -{ - return mavlink_msg_ahrs_pack_chan(system_id, component_id, chan, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw); -} - -/** - * @brief Send a ahrs message - * @param chan MAVLink channel to send the message - * - * @param omegaIx X gyro drift estimate rad/s - * @param omegaIy Y gyro drift estimate rad/s - * @param omegaIz Z gyro drift estimate rad/s - * @param accel_weight average accel_weight - * @param renorm_val average renormalisation value - * @param error_rp average error_roll_pitch value - * @param error_yaw average error_yaw value - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AHRS_LEN]; - _mav_put_float(buf, 0, omegaIx); - _mav_put_float(buf, 4, omegaIy); - _mav_put_float(buf, 8, omegaIz); - _mav_put_float(buf, 12, accel_weight); - _mav_put_float(buf, 16, renorm_val); - _mav_put_float(buf, 20, error_rp); - _mav_put_float(buf, 24, error_yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN); -#endif -#else - mavlink_ahrs_t packet; - packet.omegaIx = omegaIx; - packet.omegaIy = omegaIy; - packet.omegaIz = omegaIz; - packet.accel_weight = accel_weight; - packet.renorm_val = renorm_val; - packet.error_rp = error_rp; - packet.error_yaw = error_yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, MAVLINK_MSG_ID_AHRS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_AHRS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_ahrs_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, omegaIx); - _mav_put_float(buf, 4, omegaIy); - _mav_put_float(buf, 8, omegaIz); - _mav_put_float(buf, 12, accel_weight); - _mav_put_float(buf, 16, renorm_val); - _mav_put_float(buf, 20, error_rp); - _mav_put_float(buf, 24, error_yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN); -#endif -#else - mavlink_ahrs_t *packet = (mavlink_ahrs_t *)msgbuf; - packet->omegaIx = omegaIx; - packet->omegaIy = omegaIy; - packet->omegaIz = omegaIz; - packet->accel_weight = accel_weight; - packet->renorm_val = renorm_val; - packet->error_rp = error_rp; - packet->error_yaw = error_yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)packet, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)packet, MAVLINK_MSG_ID_AHRS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE AHRS UNPACKING - - -/** - * @brief Get field omegaIx from ahrs message - * - * @return X gyro drift estimate rad/s - */ -static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field omegaIy from ahrs message - * - * @return Y gyro drift estimate rad/s - */ -static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field omegaIz from ahrs message - * - * @return Z gyro drift estimate rad/s - */ -static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field accel_weight from ahrs message - * - * @return average accel_weight - */ -static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field renorm_val from ahrs message - * - * @return average renormalisation value - */ -static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field error_rp from ahrs message - * - * @return average error_roll_pitch value - */ -static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field error_yaw from ahrs message - * - * @return average error_yaw value - */ -static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Decode a ahrs message into a struct - * - * @param msg The message to decode - * @param ahrs C-struct to decode the message contents into - */ -static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs) -{ -#if MAVLINK_NEED_BYTE_SWAP - ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg); - ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg); - ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg); - ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg); - ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg); - ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg); - ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg); -#else - memcpy(ahrs, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h deleted file mode 100644 index 33e006688..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE AHRS2 PACKING - -#define MAVLINK_MSG_ID_AHRS2 178 - -typedef struct __mavlink_ahrs2_t -{ - float roll; ///< Roll angle (rad) - float pitch; ///< Pitch angle (rad) - float yaw; ///< Yaw angle (rad) - float altitude; ///< Altitude (MSL) - int32_t lat; ///< Latitude in degrees * 1E7 - int32_t lng; ///< Longitude in degrees * 1E7 -} mavlink_ahrs2_t; - -#define MAVLINK_MSG_ID_AHRS2_LEN 24 -#define MAVLINK_MSG_ID_178_LEN 24 - -#define MAVLINK_MSG_ID_AHRS2_CRC 47 -#define MAVLINK_MSG_ID_178_CRC 47 - - - -#define MAVLINK_MESSAGE_INFO_AHRS2 { \ - "AHRS2", \ - 6, \ - { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs2_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs2_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs2_t, yaw) }, \ - { "altitude", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs2_t, altitude) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_ahrs2_t, lat) }, \ - { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_ahrs2_t, lng) }, \ - } \ -} - - -/** - * @brief Pack a ahrs2 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param altitude Altitude (MSL) - * @param lat Latitude in degrees * 1E7 - * @param lng Longitude in degrees * 1E7 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ahrs2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AHRS2_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, altitude); - _mav_put_int32_t(buf, 16, lat); - _mav_put_int32_t(buf, 20, lng); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN); -#else - mavlink_ahrs2_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.altitude = altitude; - packet.lat = lat; - packet.lng = lng; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AHRS2; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN); -#endif -} - -/** - * @brief Pack a ahrs2 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param altitude Altitude (MSL) - * @param lat Latitude in degrees * 1E7 - * @param lng Longitude in degrees * 1E7 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ahrs2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float roll,float pitch,float yaw,float altitude,int32_t lat,int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AHRS2_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, altitude); - _mav_put_int32_t(buf, 16, lat); - _mav_put_int32_t(buf, 20, lng); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN); -#else - mavlink_ahrs2_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.altitude = altitude; - packet.lat = lat; - packet.lng = lng; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AHRS2; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN); -#endif -} - -/** - * @brief Encode a ahrs2 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param ahrs2 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ahrs2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2) -{ - return mavlink_msg_ahrs2_pack(system_id, component_id, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng); -} - -/** - * @brief Encode a ahrs2 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param ahrs2 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ahrs2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2) -{ - return mavlink_msg_ahrs2_pack_chan(system_id, component_id, chan, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng); -} - -/** - * @brief Send a ahrs2 message - * @param chan MAVLink channel to send the message - * - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param altitude Altitude (MSL) - * @param lat Latitude in degrees * 1E7 - * @param lng Longitude in degrees * 1E7 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ahrs2_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AHRS2_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, altitude); - _mav_put_int32_t(buf, 16, lat); - _mav_put_int32_t(buf, 20, lng); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN); -#endif -#else - mavlink_ahrs2_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.altitude = altitude; - packet.lat = lat; - packet.lng = lng; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_AHRS2_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_ahrs2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, altitude); - _mav_put_int32_t(buf, 16, lat); - _mav_put_int32_t(buf, 20, lng); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN); -#endif -#else - mavlink_ahrs2_t *packet = (mavlink_ahrs2_t *)msgbuf; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - packet->altitude = altitude; - packet->lat = lat; - packet->lng = lng; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)packet, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)packet, MAVLINK_MSG_ID_AHRS2_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE AHRS2 UNPACKING - - -/** - * @brief Get field roll from ahrs2 message - * - * @return Roll angle (rad) - */ -static inline float mavlink_msg_ahrs2_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field pitch from ahrs2 message - * - * @return Pitch angle (rad) - */ -static inline float mavlink_msg_ahrs2_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field yaw from ahrs2 message - * - * @return Yaw angle (rad) - */ -static inline float mavlink_msg_ahrs2_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field altitude from ahrs2 message - * - * @return Altitude (MSL) - */ -static inline float mavlink_msg_ahrs2_get_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field lat from ahrs2 message - * - * @return Latitude in degrees * 1E7 - */ -static inline int32_t mavlink_msg_ahrs2_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 16); -} - -/** - * @brief Get field lng from ahrs2 message - * - * @return Longitude in degrees * 1E7 - */ -static inline int32_t mavlink_msg_ahrs2_get_lng(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 20); -} - -/** - * @brief Decode a ahrs2 message into a struct - * - * @param msg The message to decode - * @param ahrs2 C-struct to decode the message contents into - */ -static inline void mavlink_msg_ahrs2_decode(const mavlink_message_t* msg, mavlink_ahrs2_t* ahrs2) -{ -#if MAVLINK_NEED_BYTE_SWAP - ahrs2->roll = mavlink_msg_ahrs2_get_roll(msg); - ahrs2->pitch = mavlink_msg_ahrs2_get_pitch(msg); - ahrs2->yaw = mavlink_msg_ahrs2_get_yaw(msg); - ahrs2->altitude = mavlink_msg_ahrs2_get_altitude(msg); - ahrs2->lat = mavlink_msg_ahrs2_get_lat(msg); - ahrs2->lng = mavlink_msg_ahrs2_get_lng(msg); -#else - memcpy(ahrs2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS2_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h deleted file mode 100644 index 521831c8f..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h +++ /dev/null @@ -1,473 +0,0 @@ -// MESSAGE AIRSPEED_AUTOCAL PACKING - -#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL 174 - -typedef struct __mavlink_airspeed_autocal_t -{ - float vx; ///< GPS velocity north m/s - float vy; ///< GPS velocity east m/s - float vz; ///< GPS velocity down m/s - float diff_pressure; ///< Differential pressure pascals - float EAS2TAS; ///< Estimated to true airspeed ratio - float ratio; ///< Airspeed ratio - float state_x; ///< EKF state x - float state_y; ///< EKF state y - float state_z; ///< EKF state z - float Pax; ///< EKF Pax - float Pby; ///< EKF Pby - float Pcz; ///< EKF Pcz -} mavlink_airspeed_autocal_t; - -#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN 48 -#define MAVLINK_MSG_ID_174_LEN 48 - -#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC 167 -#define MAVLINK_MSG_ID_174_CRC 167 - - - -#define MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL { \ - "AIRSPEED_AUTOCAL", \ - 12, \ - { { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_airspeed_autocal_t, vx) }, \ - { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_airspeed_autocal_t, vy) }, \ - { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_airspeed_autocal_t, vz) }, \ - { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_airspeed_autocal_t, diff_pressure) }, \ - { "EAS2TAS", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_airspeed_autocal_t, EAS2TAS) }, \ - { "ratio", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_airspeed_autocal_t, ratio) }, \ - { "state_x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_airspeed_autocal_t, state_x) }, \ - { "state_y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_airspeed_autocal_t, state_y) }, \ - { "state_z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_airspeed_autocal_t, state_z) }, \ - { "Pax", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_airspeed_autocal_t, Pax) }, \ - { "Pby", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_airspeed_autocal_t, Pby) }, \ - { "Pcz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_airspeed_autocal_t, Pcz) }, \ - } \ -} - - -/** - * @brief Pack a airspeed_autocal message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param vx GPS velocity north m/s - * @param vy GPS velocity east m/s - * @param vz GPS velocity down m/s - * @param diff_pressure Differential pressure pascals - * @param EAS2TAS Estimated to true airspeed ratio - * @param ratio Airspeed ratio - * @param state_x EKF state x - * @param state_y EKF state y - * @param state_z EKF state z - * @param Pax EKF Pax - * @param Pby EKF Pby - * @param Pcz EKF Pcz - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_airspeed_autocal_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; - _mav_put_float(buf, 0, vx); - _mav_put_float(buf, 4, vy); - _mav_put_float(buf, 8, vz); - _mav_put_float(buf, 12, diff_pressure); - _mav_put_float(buf, 16, EAS2TAS); - _mav_put_float(buf, 20, ratio); - _mav_put_float(buf, 24, state_x); - _mav_put_float(buf, 28, state_y); - _mav_put_float(buf, 32, state_z); - _mav_put_float(buf, 36, Pax); - _mav_put_float(buf, 40, Pby); - _mav_put_float(buf, 44, Pcz); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#else - mavlink_airspeed_autocal_t packet; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.diff_pressure = diff_pressure; - packet.EAS2TAS = EAS2TAS; - packet.ratio = ratio; - packet.state_x = state_x; - packet.state_y = state_y; - packet.state_z = state_z; - packet.Pax = Pax; - packet.Pby = Pby; - packet.Pcz = Pcz; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif -} - -/** - * @brief Pack a airspeed_autocal message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param vx GPS velocity north m/s - * @param vy GPS velocity east m/s - * @param vz GPS velocity down m/s - * @param diff_pressure Differential pressure pascals - * @param EAS2TAS Estimated to true airspeed ratio - * @param ratio Airspeed ratio - * @param state_x EKF state x - * @param state_y EKF state y - * @param state_z EKF state z - * @param Pax EKF Pax - * @param Pby EKF Pby - * @param Pcz EKF Pcz - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_airspeed_autocal_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float vx,float vy,float vz,float diff_pressure,float EAS2TAS,float ratio,float state_x,float state_y,float state_z,float Pax,float Pby,float Pcz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; - _mav_put_float(buf, 0, vx); - _mav_put_float(buf, 4, vy); - _mav_put_float(buf, 8, vz); - _mav_put_float(buf, 12, diff_pressure); - _mav_put_float(buf, 16, EAS2TAS); - _mav_put_float(buf, 20, ratio); - _mav_put_float(buf, 24, state_x); - _mav_put_float(buf, 28, state_y); - _mav_put_float(buf, 32, state_z); - _mav_put_float(buf, 36, Pax); - _mav_put_float(buf, 40, Pby); - _mav_put_float(buf, 44, Pcz); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#else - mavlink_airspeed_autocal_t packet; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.diff_pressure = diff_pressure; - packet.EAS2TAS = EAS2TAS; - packet.ratio = ratio; - packet.state_x = state_x; - packet.state_y = state_y; - packet.state_z = state_z; - packet.Pax = Pax; - packet.Pby = Pby; - packet.Pcz = Pcz; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif -} - -/** - * @brief Encode a airspeed_autocal struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param airspeed_autocal C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_airspeed_autocal_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal) -{ - return mavlink_msg_airspeed_autocal_pack(system_id, component_id, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz); -} - -/** - * @brief Encode a airspeed_autocal struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param airspeed_autocal C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_airspeed_autocal_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal) -{ - return mavlink_msg_airspeed_autocal_pack_chan(system_id, component_id, chan, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz); -} - -/** - * @brief Send a airspeed_autocal message - * @param chan MAVLink channel to send the message - * - * @param vx GPS velocity north m/s - * @param vy GPS velocity east m/s - * @param vz GPS velocity down m/s - * @param diff_pressure Differential pressure pascals - * @param EAS2TAS Estimated to true airspeed ratio - * @param ratio Airspeed ratio - * @param state_x EKF state x - * @param state_y EKF state y - * @param state_z EKF state z - * @param Pax EKF Pax - * @param Pby EKF Pby - * @param Pcz EKF Pcz - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_airspeed_autocal_send(mavlink_channel_t chan, float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; - _mav_put_float(buf, 0, vx); - _mav_put_float(buf, 4, vy); - _mav_put_float(buf, 8, vz); - _mav_put_float(buf, 12, diff_pressure); - _mav_put_float(buf, 16, EAS2TAS); - _mav_put_float(buf, 20, ratio); - _mav_put_float(buf, 24, state_x); - _mav_put_float(buf, 28, state_y); - _mav_put_float(buf, 32, state_z); - _mav_put_float(buf, 36, Pax); - _mav_put_float(buf, 40, Pby); - _mav_put_float(buf, 44, Pcz); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif -#else - mavlink_airspeed_autocal_t packet; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.diff_pressure = diff_pressure; - packet.EAS2TAS = EAS2TAS; - packet.ratio = ratio; - packet.state_x = state_x; - packet.state_y = state_y; - packet.state_z = state_z; - packet.Pax = Pax; - packet.Pby = Pby; - packet.Pcz = Pcz; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_airspeed_autocal_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, vx); - _mav_put_float(buf, 4, vy); - _mav_put_float(buf, 8, vz); - _mav_put_float(buf, 12, diff_pressure); - _mav_put_float(buf, 16, EAS2TAS); - _mav_put_float(buf, 20, ratio); - _mav_put_float(buf, 24, state_x); - _mav_put_float(buf, 28, state_y); - _mav_put_float(buf, 32, state_z); - _mav_put_float(buf, 36, Pax); - _mav_put_float(buf, 40, Pby); - _mav_put_float(buf, 44, Pcz); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif -#else - mavlink_airspeed_autocal_t *packet = (mavlink_airspeed_autocal_t *)msgbuf; - packet->vx = vx; - packet->vy = vy; - packet->vz = vz; - packet->diff_pressure = diff_pressure; - packet->EAS2TAS = EAS2TAS; - packet->ratio = ratio; - packet->state_x = state_x; - packet->state_y = state_y; - packet->state_z = state_z; - packet->Pax = Pax; - packet->Pby = Pby; - packet->Pcz = Pcz; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE AIRSPEED_AUTOCAL UNPACKING - - -/** - * @brief Get field vx from airspeed_autocal message - * - * @return GPS velocity north m/s - */ -static inline float mavlink_msg_airspeed_autocal_get_vx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field vy from airspeed_autocal message - * - * @return GPS velocity east m/s - */ -static inline float mavlink_msg_airspeed_autocal_get_vy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field vz from airspeed_autocal message - * - * @return GPS velocity down m/s - */ -static inline float mavlink_msg_airspeed_autocal_get_vz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field diff_pressure from airspeed_autocal message - * - * @return Differential pressure pascals - */ -static inline float mavlink_msg_airspeed_autocal_get_diff_pressure(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field EAS2TAS from airspeed_autocal message - * - * @return Estimated to true airspeed ratio - */ -static inline float mavlink_msg_airspeed_autocal_get_EAS2TAS(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field ratio from airspeed_autocal message - * - * @return Airspeed ratio - */ -static inline float mavlink_msg_airspeed_autocal_get_ratio(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field state_x from airspeed_autocal message - * - * @return EKF state x - */ -static inline float mavlink_msg_airspeed_autocal_get_state_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field state_y from airspeed_autocal message - * - * @return EKF state y - */ -static inline float mavlink_msg_airspeed_autocal_get_state_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field state_z from airspeed_autocal message - * - * @return EKF state z - */ -static inline float mavlink_msg_airspeed_autocal_get_state_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field Pax from airspeed_autocal message - * - * @return EKF Pax - */ -static inline float mavlink_msg_airspeed_autocal_get_Pax(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 36); -} - -/** - * @brief Get field Pby from airspeed_autocal message - * - * @return EKF Pby - */ -static inline float mavlink_msg_airspeed_autocal_get_Pby(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 40); -} - -/** - * @brief Get field Pcz from airspeed_autocal message - * - * @return EKF Pcz - */ -static inline float mavlink_msg_airspeed_autocal_get_Pcz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 44); -} - -/** - * @brief Decode a airspeed_autocal message into a struct - * - * @param msg The message to decode - * @param airspeed_autocal C-struct to decode the message contents into - */ -static inline void mavlink_msg_airspeed_autocal_decode(const mavlink_message_t* msg, mavlink_airspeed_autocal_t* airspeed_autocal) -{ -#if MAVLINK_NEED_BYTE_SWAP - airspeed_autocal->vx = mavlink_msg_airspeed_autocal_get_vx(msg); - airspeed_autocal->vy = mavlink_msg_airspeed_autocal_get_vy(msg); - airspeed_autocal->vz = mavlink_msg_airspeed_autocal_get_vz(msg); - airspeed_autocal->diff_pressure = mavlink_msg_airspeed_autocal_get_diff_pressure(msg); - airspeed_autocal->EAS2TAS = mavlink_msg_airspeed_autocal_get_EAS2TAS(msg); - airspeed_autocal->ratio = mavlink_msg_airspeed_autocal_get_ratio(msg); - airspeed_autocal->state_x = mavlink_msg_airspeed_autocal_get_state_x(msg); - airspeed_autocal->state_y = mavlink_msg_airspeed_autocal_get_state_y(msg); - airspeed_autocal->state_z = mavlink_msg_airspeed_autocal_get_state_z(msg); - airspeed_autocal->Pax = mavlink_msg_airspeed_autocal_get_Pax(msg); - airspeed_autocal->Pby = mavlink_msg_airspeed_autocal_get_Pby(msg); - airspeed_autocal->Pcz = mavlink_msg_airspeed_autocal_get_Pcz(msg); -#else - memcpy(airspeed_autocal, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h deleted file mode 100644 index 3c18e644e..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE AP_ADC PACKING - -#define MAVLINK_MSG_ID_AP_ADC 153 - -typedef struct __mavlink_ap_adc_t -{ - uint16_t adc1; ///< ADC output 1 - uint16_t adc2; ///< ADC output 2 - uint16_t adc3; ///< ADC output 3 - uint16_t adc4; ///< ADC output 4 - uint16_t adc5; ///< ADC output 5 - uint16_t adc6; ///< ADC output 6 -} mavlink_ap_adc_t; - -#define MAVLINK_MSG_ID_AP_ADC_LEN 12 -#define MAVLINK_MSG_ID_153_LEN 12 - -#define MAVLINK_MSG_ID_AP_ADC_CRC 188 -#define MAVLINK_MSG_ID_153_CRC 188 - - - -#define MAVLINK_MESSAGE_INFO_AP_ADC { \ - "AP_ADC", \ - 6, \ - { { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_adc_t, adc1) }, \ - { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_adc_t, adc2) }, \ - { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_adc_t, adc3) }, \ - { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_adc_t, adc4) }, \ - { "adc5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_ap_adc_t, adc5) }, \ - { "adc6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_ap_adc_t, adc6) }, \ - } \ -} - - -/** - * @brief Pack a ap_adc message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AP_ADC_LEN]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AP_ADC_LEN); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AP_ADC; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif -} - -/** - * @brief Pack a ap_adc message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t adc5,uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AP_ADC_LEN]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AP_ADC_LEN); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AP_ADC; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif -} - -/** - * @brief Encode a ap_adc struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param ap_adc C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) -{ - return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); -} - -/** - * @brief Encode a ap_adc struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param ap_adc C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ap_adc_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) -{ - return mavlink_msg_ap_adc_pack_chan(system_id, component_id, chan, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); -} - -/** - * @brief Send a ap_adc message - * @param chan MAVLink channel to send the message - * - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AP_ADC_LEN]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_AP_ADC_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_ap_adc_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif -#else - mavlink_ap_adc_t *packet = (mavlink_ap_adc_t *)msgbuf; - packet->adc1 = adc1; - packet->adc2 = adc2; - packet->adc3 = adc3; - packet->adc4 = adc4; - packet->adc5 = adc5; - packet->adc6 = adc6; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)packet, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)packet, MAVLINK_MSG_ID_AP_ADC_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE AP_ADC UNPACKING - - -/** - * @brief Get field adc1 from ap_adc message - * - * @return ADC output 1 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field adc2 from ap_adc message - * - * @return ADC output 2 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field adc3 from ap_adc message - * - * @return ADC output 3 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field adc4 from ap_adc message - * - * @return ADC output 4 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field adc5 from ap_adc message - * - * @return ADC output 5 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field adc6 from ap_adc message - * - * @return ADC output 6 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Decode a ap_adc message into a struct - * - * @param msg The message to decode - * @param ap_adc C-struct to decode the message contents into - */ -static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavlink_ap_adc_t* ap_adc) -{ -#if MAVLINK_NEED_BYTE_SWAP - ap_adc->adc1 = mavlink_msg_ap_adc_get_adc1(msg); - ap_adc->adc2 = mavlink_msg_ap_adc_get_adc2(msg); - ap_adc->adc3 = mavlink_msg_ap_adc_get_adc3(msg); - ap_adc->adc4 = mavlink_msg_ap_adc_get_adc4(msg); - ap_adc->adc5 = mavlink_msg_ap_adc_get_adc5(msg); - ap_adc->adc6 = mavlink_msg_ap_adc_get_adc6(msg); -#else - memcpy(ap_adc, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AP_ADC_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h deleted file mode 100644 index 965545988..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE COMPASSMOT_STATUS PACKING - -#define MAVLINK_MSG_ID_COMPASSMOT_STATUS 177 - -typedef struct __mavlink_compassmot_status_t -{ - float current; ///< current (amps) - float CompensationX; ///< Motor Compensation X - float CompensationY; ///< Motor Compensation Y - float CompensationZ; ///< Motor Compensation Z - uint16_t throttle; ///< throttle (percent*10) - uint16_t interference; ///< interference (percent) -} mavlink_compassmot_status_t; - -#define MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN 20 -#define MAVLINK_MSG_ID_177_LEN 20 - -#define MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC 240 -#define MAVLINK_MSG_ID_177_CRC 240 - - - -#define MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS { \ - "COMPASSMOT_STATUS", \ - 6, \ - { { "current", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_compassmot_status_t, current) }, \ - { "CompensationX", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_compassmot_status_t, CompensationX) }, \ - { "CompensationY", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_compassmot_status_t, CompensationY) }, \ - { "CompensationZ", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_compassmot_status_t, CompensationZ) }, \ - { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_compassmot_status_t, throttle) }, \ - { "interference", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_compassmot_status_t, interference) }, \ - } \ -} - - -/** - * @brief Pack a compassmot_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param throttle throttle (percent*10) - * @param current current (amps) - * @param interference interference (percent) - * @param CompensationX Motor Compensation X - * @param CompensationY Motor Compensation Y - * @param CompensationZ Motor Compensation Z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_compassmot_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN]; - _mav_put_float(buf, 0, current); - _mav_put_float(buf, 4, CompensationX); - _mav_put_float(buf, 8, CompensationY); - _mav_put_float(buf, 12, CompensationZ); - _mav_put_uint16_t(buf, 16, throttle); - _mav_put_uint16_t(buf, 18, interference); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); -#else - mavlink_compassmot_status_t packet; - packet.current = current; - packet.CompensationX = CompensationX; - packet.CompensationY = CompensationY; - packet.CompensationZ = CompensationZ; - packet.throttle = throttle; - packet.interference = interference; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_COMPASSMOT_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); -#endif -} - -/** - * @brief Pack a compassmot_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param throttle throttle (percent*10) - * @param current current (amps) - * @param interference interference (percent) - * @param CompensationX Motor Compensation X - * @param CompensationY Motor Compensation Y - * @param CompensationZ Motor Compensation Z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_compassmot_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t throttle,float current,uint16_t interference,float CompensationX,float CompensationY,float CompensationZ) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN]; - _mav_put_float(buf, 0, current); - _mav_put_float(buf, 4, CompensationX); - _mav_put_float(buf, 8, CompensationY); - _mav_put_float(buf, 12, CompensationZ); - _mav_put_uint16_t(buf, 16, throttle); - _mav_put_uint16_t(buf, 18, interference); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); -#else - mavlink_compassmot_status_t packet; - packet.current = current; - packet.CompensationX = CompensationX; - packet.CompensationY = CompensationY; - packet.CompensationZ = CompensationZ; - packet.throttle = throttle; - packet.interference = interference; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_COMPASSMOT_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); -#endif -} - -/** - * @brief Encode a compassmot_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param compassmot_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_compassmot_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_compassmot_status_t* compassmot_status) -{ - return mavlink_msg_compassmot_status_pack(system_id, component_id, msg, compassmot_status->throttle, compassmot_status->current, compassmot_status->interference, compassmot_status->CompensationX, compassmot_status->CompensationY, compassmot_status->CompensationZ); -} - -/** - * @brief Encode a compassmot_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param compassmot_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_compassmot_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_compassmot_status_t* compassmot_status) -{ - return mavlink_msg_compassmot_status_pack_chan(system_id, component_id, chan, msg, compassmot_status->throttle, compassmot_status->current, compassmot_status->interference, compassmot_status->CompensationX, compassmot_status->CompensationY, compassmot_status->CompensationZ); -} - -/** - * @brief Send a compassmot_status message - * @param chan MAVLink channel to send the message - * - * @param throttle throttle (percent*10) - * @param current current (amps) - * @param interference interference (percent) - * @param CompensationX Motor Compensation X - * @param CompensationY Motor Compensation Y - * @param CompensationZ Motor Compensation Z - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_compassmot_status_send(mavlink_channel_t chan, uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN]; - _mav_put_float(buf, 0, current); - _mav_put_float(buf, 4, CompensationX); - _mav_put_float(buf, 8, CompensationY); - _mav_put_float(buf, 12, CompensationZ); - _mav_put_uint16_t(buf, 16, throttle); - _mav_put_uint16_t(buf, 18, interference); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); -#endif -#else - mavlink_compassmot_status_t packet; - packet.current = current; - packet.CompensationX = CompensationX; - packet.CompensationY = CompensationY; - packet.CompensationZ = CompensationZ; - packet.throttle = throttle; - packet.interference = interference; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_compassmot_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, current); - _mav_put_float(buf, 4, CompensationX); - _mav_put_float(buf, 8, CompensationY); - _mav_put_float(buf, 12, CompensationZ); - _mav_put_uint16_t(buf, 16, throttle); - _mav_put_uint16_t(buf, 18, interference); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); -#endif -#else - mavlink_compassmot_status_t *packet = (mavlink_compassmot_status_t *)msgbuf; - packet->current = current; - packet->CompensationX = CompensationX; - packet->CompensationY = CompensationY; - packet->CompensationZ = CompensationZ; - packet->throttle = throttle; - packet->interference = interference; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE COMPASSMOT_STATUS UNPACKING - - -/** - * @brief Get field throttle from compassmot_status message - * - * @return throttle (percent*10) - */ -static inline uint16_t mavlink_msg_compassmot_status_get_throttle(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field current from compassmot_status message - * - * @return current (amps) - */ -static inline float mavlink_msg_compassmot_status_get_current(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field interference from compassmot_status message - * - * @return interference (percent) - */ -static inline uint16_t mavlink_msg_compassmot_status_get_interference(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 18); -} - -/** - * @brief Get field CompensationX from compassmot_status message - * - * @return Motor Compensation X - */ -static inline float mavlink_msg_compassmot_status_get_CompensationX(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field CompensationY from compassmot_status message - * - * @return Motor Compensation Y - */ -static inline float mavlink_msg_compassmot_status_get_CompensationY(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field CompensationZ from compassmot_status message - * - * @return Motor Compensation Z - */ -static inline float mavlink_msg_compassmot_status_get_CompensationZ(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a compassmot_status message into a struct - * - * @param msg The message to decode - * @param compassmot_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_compassmot_status_decode(const mavlink_message_t* msg, mavlink_compassmot_status_t* compassmot_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - compassmot_status->current = mavlink_msg_compassmot_status_get_current(msg); - compassmot_status->CompensationX = mavlink_msg_compassmot_status_get_CompensationX(msg); - compassmot_status->CompensationY = mavlink_msg_compassmot_status_get_CompensationY(msg); - compassmot_status->CompensationZ = mavlink_msg_compassmot_status_get_CompensationZ(msg); - compassmot_status->throttle = mavlink_msg_compassmot_status_get_throttle(msg); - compassmot_status->interference = mavlink_msg_compassmot_status_get_interference(msg); -#else - memcpy(compassmot_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h deleted file mode 100644 index 7bad1b23d..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h +++ /dev/null @@ -1,249 +0,0 @@ -// MESSAGE DATA16 PACKING - -#define MAVLINK_MSG_ID_DATA16 169 - -typedef struct __mavlink_data16_t -{ - uint8_t type; ///< data type - uint8_t len; ///< data length - uint8_t data[16]; ///< raw data -} mavlink_data16_t; - -#define MAVLINK_MSG_ID_DATA16_LEN 18 -#define MAVLINK_MSG_ID_169_LEN 18 - -#define MAVLINK_MSG_ID_DATA16_CRC 234 -#define MAVLINK_MSG_ID_169_CRC 234 - -#define MAVLINK_MSG_DATA16_FIELD_DATA_LEN 16 - -#define MAVLINK_MESSAGE_INFO_DATA16 { \ - "DATA16", \ - 3, \ - { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data16_t, type) }, \ - { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data16_t, len) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 16, 2, offsetof(mavlink_data16_t, data) }, \ - } \ -} - - -/** - * @brief Pack a data16 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data16_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA16_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA16_LEN); -#else - mavlink_data16_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA16; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA16_LEN); -#endif -} - -/** - * @brief Pack a data16 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data16_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t len,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA16_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA16_LEN); -#else - mavlink_data16_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA16; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA16_LEN); -#endif -} - -/** - * @brief Encode a data16 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param data16 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data16_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data16_t* data16) -{ - return mavlink_msg_data16_pack(system_id, component_id, msg, data16->type, data16->len, data16->data); -} - -/** - * @brief Encode a data16 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param data16 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data16_t* data16) -{ - return mavlink_msg_data16_pack_chan(system_id, component_id, chan, msg, data16->type, data16->len, data16->data); -} - -/** - * @brief Send a data16 message - * @param chan MAVLink channel to send the message - * - * @param type data type - * @param len data length - * @param data raw data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data16_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA16_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN); -#endif -#else - mavlink_data16_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)&packet, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)&packet, MAVLINK_MSG_ID_DATA16_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_DATA16_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_data16_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN); -#endif -#else - mavlink_data16_t *packet = (mavlink_data16_t *)msgbuf; - packet->type = type; - packet->len = len; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)packet, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)packet, MAVLINK_MSG_ID_DATA16_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE DATA16 UNPACKING - - -/** - * @brief Get field type from data16 message - * - * @return data type - */ -static inline uint8_t mavlink_msg_data16_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field len from data16 message - * - * @return data length - */ -static inline uint8_t mavlink_msg_data16_get_len(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field data from data16 message - * - * @return raw data - */ -static inline uint16_t mavlink_msg_data16_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 16, 2); -} - -/** - * @brief Decode a data16 message into a struct - * - * @param msg The message to decode - * @param data16 C-struct to decode the message contents into - */ -static inline void mavlink_msg_data16_decode(const mavlink_message_t* msg, mavlink_data16_t* data16) -{ -#if MAVLINK_NEED_BYTE_SWAP - data16->type = mavlink_msg_data16_get_type(msg); - data16->len = mavlink_msg_data16_get_len(msg); - mavlink_msg_data16_get_data(msg, data16->data); -#else - memcpy(data16, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA16_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h deleted file mode 100644 index df7a2ec80..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h +++ /dev/null @@ -1,249 +0,0 @@ -// MESSAGE DATA32 PACKING - -#define MAVLINK_MSG_ID_DATA32 170 - -typedef struct __mavlink_data32_t -{ - uint8_t type; ///< data type - uint8_t len; ///< data length - uint8_t data[32]; ///< raw data -} mavlink_data32_t; - -#define MAVLINK_MSG_ID_DATA32_LEN 34 -#define MAVLINK_MSG_ID_170_LEN 34 - -#define MAVLINK_MSG_ID_DATA32_CRC 73 -#define MAVLINK_MSG_ID_170_CRC 73 - -#define MAVLINK_MSG_DATA32_FIELD_DATA_LEN 32 - -#define MAVLINK_MESSAGE_INFO_DATA32 { \ - "DATA32", \ - 3, \ - { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data32_t, type) }, \ - { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data32_t, len) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 32, 2, offsetof(mavlink_data32_t, data) }, \ - } \ -} - - -/** - * @brief Pack a data32 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data32_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA32_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA32_LEN); -#else - mavlink_data32_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA32; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA32_LEN); -#endif -} - -/** - * @brief Pack a data32 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data32_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t len,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA32_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA32_LEN); -#else - mavlink_data32_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA32; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA32_LEN); -#endif -} - -/** - * @brief Encode a data32 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param data32 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data32_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data32_t* data32) -{ - return mavlink_msg_data32_pack(system_id, component_id, msg, data32->type, data32->len, data32->data); -} - -/** - * @brief Encode a data32 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param data32 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data32_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data32_t* data32) -{ - return mavlink_msg_data32_pack_chan(system_id, component_id, chan, msg, data32->type, data32->len, data32->data); -} - -/** - * @brief Send a data32 message - * @param chan MAVLink channel to send the message - * - * @param type data type - * @param len data length - * @param data raw data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data32_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA32_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN); -#endif -#else - mavlink_data32_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)&packet, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)&packet, MAVLINK_MSG_ID_DATA32_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_DATA32_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_data32_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN); -#endif -#else - mavlink_data32_t *packet = (mavlink_data32_t *)msgbuf; - packet->type = type; - packet->len = len; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)packet, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)packet, MAVLINK_MSG_ID_DATA32_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE DATA32 UNPACKING - - -/** - * @brief Get field type from data32 message - * - * @return data type - */ -static inline uint8_t mavlink_msg_data32_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field len from data32 message - * - * @return data length - */ -static inline uint8_t mavlink_msg_data32_get_len(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field data from data32 message - * - * @return raw data - */ -static inline uint16_t mavlink_msg_data32_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 32, 2); -} - -/** - * @brief Decode a data32 message into a struct - * - * @param msg The message to decode - * @param data32 C-struct to decode the message contents into - */ -static inline void mavlink_msg_data32_decode(const mavlink_message_t* msg, mavlink_data32_t* data32) -{ -#if MAVLINK_NEED_BYTE_SWAP - data32->type = mavlink_msg_data32_get_type(msg); - data32->len = mavlink_msg_data32_get_len(msg); - mavlink_msg_data32_get_data(msg, data32->data); -#else - memcpy(data32, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA32_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h deleted file mode 100644 index c354f8908..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h +++ /dev/null @@ -1,249 +0,0 @@ -// MESSAGE DATA64 PACKING - -#define MAVLINK_MSG_ID_DATA64 171 - -typedef struct __mavlink_data64_t -{ - uint8_t type; ///< data type - uint8_t len; ///< data length - uint8_t data[64]; ///< raw data -} mavlink_data64_t; - -#define MAVLINK_MSG_ID_DATA64_LEN 66 -#define MAVLINK_MSG_ID_171_LEN 66 - -#define MAVLINK_MSG_ID_DATA64_CRC 181 -#define MAVLINK_MSG_ID_171_CRC 181 - -#define MAVLINK_MSG_DATA64_FIELD_DATA_LEN 64 - -#define MAVLINK_MESSAGE_INFO_DATA64 { \ - "DATA64", \ - 3, \ - { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data64_t, type) }, \ - { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data64_t, len) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 64, 2, offsetof(mavlink_data64_t, data) }, \ - } \ -} - - -/** - * @brief Pack a data64 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data64_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA64_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 64); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA64_LEN); -#else - mavlink_data64_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA64; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA64_LEN); -#endif -} - -/** - * @brief Pack a data64 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data64_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t len,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA64_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 64); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA64_LEN); -#else - mavlink_data64_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA64; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA64_LEN); -#endif -} - -/** - * @brief Encode a data64 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param data64 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data64_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data64_t* data64) -{ - return mavlink_msg_data64_pack(system_id, component_id, msg, data64->type, data64->len, data64->data); -} - -/** - * @brief Encode a data64 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param data64 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data64_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data64_t* data64) -{ - return mavlink_msg_data64_pack_chan(system_id, component_id, chan, msg, data64->type, data64->len, data64->data); -} - -/** - * @brief Send a data64 message - * @param chan MAVLink channel to send the message - * - * @param type data type - * @param len data length - * @param data raw data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data64_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA64_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 64); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN); -#endif -#else - mavlink_data64_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)&packet, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)&packet, MAVLINK_MSG_ID_DATA64_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_DATA64_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_data64_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 64); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN); -#endif -#else - mavlink_data64_t *packet = (mavlink_data64_t *)msgbuf; - packet->type = type; - packet->len = len; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*64); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)packet, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)packet, MAVLINK_MSG_ID_DATA64_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE DATA64 UNPACKING - - -/** - * @brief Get field type from data64 message - * - * @return data type - */ -static inline uint8_t mavlink_msg_data64_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field len from data64 message - * - * @return data length - */ -static inline uint8_t mavlink_msg_data64_get_len(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field data from data64 message - * - * @return raw data - */ -static inline uint16_t mavlink_msg_data64_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 64, 2); -} - -/** - * @brief Decode a data64 message into a struct - * - * @param msg The message to decode - * @param data64 C-struct to decode the message contents into - */ -static inline void mavlink_msg_data64_decode(const mavlink_message_t* msg, mavlink_data64_t* data64) -{ -#if MAVLINK_NEED_BYTE_SWAP - data64->type = mavlink_msg_data64_get_type(msg); - data64->len = mavlink_msg_data64_get_len(msg); - mavlink_msg_data64_get_data(msg, data64->data); -#else - memcpy(data64, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA64_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h deleted file mode 100644 index 634e3f81d..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h +++ /dev/null @@ -1,249 +0,0 @@ -// MESSAGE DATA96 PACKING - -#define MAVLINK_MSG_ID_DATA96 172 - -typedef struct __mavlink_data96_t -{ - uint8_t type; ///< data type - uint8_t len; ///< data length - uint8_t data[96]; ///< raw data -} mavlink_data96_t; - -#define MAVLINK_MSG_ID_DATA96_LEN 98 -#define MAVLINK_MSG_ID_172_LEN 98 - -#define MAVLINK_MSG_ID_DATA96_CRC 22 -#define MAVLINK_MSG_ID_172_CRC 22 - -#define MAVLINK_MSG_DATA96_FIELD_DATA_LEN 96 - -#define MAVLINK_MESSAGE_INFO_DATA96 { \ - "DATA96", \ - 3, \ - { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data96_t, type) }, \ - { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data96_t, len) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 96, 2, offsetof(mavlink_data96_t, data) }, \ - } \ -} - - -/** - * @brief Pack a data96 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data96_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA96_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 96); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA96_LEN); -#else - mavlink_data96_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA96; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA96_LEN); -#endif -} - -/** - * @brief Pack a data96 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type data type - * @param len data length - * @param data raw data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data96_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t len,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA96_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 96); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA96_LEN); -#else - mavlink_data96_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA96; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA96_LEN); -#endif -} - -/** - * @brief Encode a data96 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param data96 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data96_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data96_t* data96) -{ - return mavlink_msg_data96_pack(system_id, component_id, msg, data96->type, data96->len, data96->data); -} - -/** - * @brief Encode a data96 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param data96 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data96_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data96_t* data96) -{ - return mavlink_msg_data96_pack_chan(system_id, component_id, chan, msg, data96->type, data96->len, data96->data); -} - -/** - * @brief Send a data96 message - * @param chan MAVLink channel to send the message - * - * @param type data type - * @param len data length - * @param data raw data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data96_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA96_LEN]; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 96); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN); -#endif -#else - mavlink_data96_t packet; - packet.type = type; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)&packet, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)&packet, MAVLINK_MSG_ID_DATA96_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_DATA96_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_data96_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, type); - _mav_put_uint8_t(buf, 1, len); - _mav_put_uint8_t_array(buf, 2, data, 96); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN); -#endif -#else - mavlink_data96_t *packet = (mavlink_data96_t *)msgbuf; - packet->type = type; - packet->len = len; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*96); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)packet, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)packet, MAVLINK_MSG_ID_DATA96_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE DATA96 UNPACKING - - -/** - * @brief Get field type from data96 message - * - * @return data type - */ -static inline uint8_t mavlink_msg_data96_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field len from data96 message - * - * @return data length - */ -static inline uint8_t mavlink_msg_data96_get_len(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field data from data96 message - * - * @return raw data - */ -static inline uint16_t mavlink_msg_data96_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 96, 2); -} - -/** - * @brief Decode a data96 message into a struct - * - * @param msg The message to decode - * @param data96 C-struct to decode the message contents into - */ -static inline void mavlink_msg_data96_decode(const mavlink_message_t* msg, mavlink_data96_t* data96) -{ -#if MAVLINK_NEED_BYTE_SWAP - data96->type = mavlink_msg_data96_get_type(msg); - data96->len = mavlink_msg_data96_get_len(msg); - mavlink_msg_data96_get_data(msg, data96->data); -#else - memcpy(data96, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA96_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h deleted file mode 100644 index d5f9e7132..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h +++ /dev/null @@ -1,449 +0,0 @@ -// MESSAGE DIGICAM_CONFIGURE PACKING - -#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE 154 - -typedef struct __mavlink_digicam_configure_t -{ - float extra_value; ///< Correspondent value to given extra_param - uint16_t shutter_speed; ///< Divisor number //e.g. 1000 means 1/1000 (0 means ignore) - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t mode; ///< Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) - uint8_t aperture; ///< F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) - uint8_t iso; ///< ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) - uint8_t exposure_type; ///< Exposure type enumeration from 1 to N (0 means ignore) - uint8_t command_id; ///< Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - uint8_t engine_cut_off; ///< Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) - uint8_t extra_param; ///< Extra parameters enumeration (0 means ignore) -} mavlink_digicam_configure_t; - -#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN 15 -#define MAVLINK_MSG_ID_154_LEN 15 - -#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC 84 -#define MAVLINK_MSG_ID_154_CRC 84 - - - -#define MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE { \ - "DIGICAM_CONFIGURE", \ - 11, \ - { { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_digicam_configure_t, extra_value) }, \ - { "shutter_speed", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_digicam_configure_t, shutter_speed) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_configure_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_configure_t, target_component) }, \ - { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_digicam_configure_t, mode) }, \ - { "aperture", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_digicam_configure_t, aperture) }, \ - { "iso", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_digicam_configure_t, iso) }, \ - { "exposure_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_digicam_configure_t, exposure_type) }, \ - { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_digicam_configure_t, command_id) }, \ - { "engine_cut_off", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_digicam_configure_t, engine_cut_off) }, \ - { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_digicam_configure_t, extra_param) }, \ - } \ -} - - -/** - * @brief Pack a digicam_configure message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) - * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore) - * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) - * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) - * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore) - * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) - * @param extra_param Extra parameters enumeration (0 means ignore) - * @param extra_value Correspondent value to given extra_param - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_digicam_configure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN]; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint16_t(buf, 4, shutter_speed); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - _mav_put_uint8_t(buf, 8, mode); - _mav_put_uint8_t(buf, 9, aperture); - _mav_put_uint8_t(buf, 10, iso); - _mav_put_uint8_t(buf, 11, exposure_type); - _mav_put_uint8_t(buf, 12, command_id); - _mav_put_uint8_t(buf, 13, engine_cut_off); - _mav_put_uint8_t(buf, 14, extra_param); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#else - mavlink_digicam_configure_t packet; - packet.extra_value = extra_value; - packet.shutter_speed = shutter_speed; - packet.target_system = target_system; - packet.target_component = target_component; - packet.mode = mode; - packet.aperture = aperture; - packet.iso = iso; - packet.exposure_type = exposure_type; - packet.command_id = command_id; - packet.engine_cut_off = engine_cut_off; - packet.extra_param = extra_param; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif -} - -/** - * @brief Pack a digicam_configure message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) - * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore) - * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) - * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) - * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore) - * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) - * @param extra_param Extra parameters enumeration (0 means ignore) - * @param extra_value Correspondent value to given extra_param - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_digicam_configure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t mode,uint16_t shutter_speed,uint8_t aperture,uint8_t iso,uint8_t exposure_type,uint8_t command_id,uint8_t engine_cut_off,uint8_t extra_param,float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN]; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint16_t(buf, 4, shutter_speed); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - _mav_put_uint8_t(buf, 8, mode); - _mav_put_uint8_t(buf, 9, aperture); - _mav_put_uint8_t(buf, 10, iso); - _mav_put_uint8_t(buf, 11, exposure_type); - _mav_put_uint8_t(buf, 12, command_id); - _mav_put_uint8_t(buf, 13, engine_cut_off); - _mav_put_uint8_t(buf, 14, extra_param); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#else - mavlink_digicam_configure_t packet; - packet.extra_value = extra_value; - packet.shutter_speed = shutter_speed; - packet.target_system = target_system; - packet.target_component = target_component; - packet.mode = mode; - packet.aperture = aperture; - packet.iso = iso; - packet.exposure_type = exposure_type; - packet.command_id = command_id; - packet.engine_cut_off = engine_cut_off; - packet.extra_param = extra_param; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif -} - -/** - * @brief Encode a digicam_configure struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param digicam_configure C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_digicam_configure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure) -{ - return mavlink_msg_digicam_configure_pack(system_id, component_id, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value); -} - -/** - * @brief Encode a digicam_configure struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param digicam_configure C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_digicam_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure) -{ - return mavlink_msg_digicam_configure_pack_chan(system_id, component_id, chan, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value); -} - -/** - * @brief Send a digicam_configure message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) - * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore) - * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) - * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) - * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore) - * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) - * @param extra_param Extra parameters enumeration (0 means ignore) - * @param extra_value Correspondent value to given extra_param - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_digicam_configure_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN]; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint16_t(buf, 4, shutter_speed); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - _mav_put_uint8_t(buf, 8, mode); - _mav_put_uint8_t(buf, 9, aperture); - _mav_put_uint8_t(buf, 10, iso); - _mav_put_uint8_t(buf, 11, exposure_type); - _mav_put_uint8_t(buf, 12, command_id); - _mav_put_uint8_t(buf, 13, engine_cut_off); - _mav_put_uint8_t(buf, 14, extra_param); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif -#else - mavlink_digicam_configure_t packet; - packet.extra_value = extra_value; - packet.shutter_speed = shutter_speed; - packet.target_system = target_system; - packet.target_component = target_component; - packet.mode = mode; - packet.aperture = aperture; - packet.iso = iso; - packet.exposure_type = exposure_type; - packet.command_id = command_id; - packet.engine_cut_off = engine_cut_off; - packet.extra_param = extra_param; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_digicam_configure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint16_t(buf, 4, shutter_speed); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - _mav_put_uint8_t(buf, 8, mode); - _mav_put_uint8_t(buf, 9, aperture); - _mav_put_uint8_t(buf, 10, iso); - _mav_put_uint8_t(buf, 11, exposure_type); - _mav_put_uint8_t(buf, 12, command_id); - _mav_put_uint8_t(buf, 13, engine_cut_off); - _mav_put_uint8_t(buf, 14, extra_param); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif -#else - mavlink_digicam_configure_t *packet = (mavlink_digicam_configure_t *)msgbuf; - packet->extra_value = extra_value; - packet->shutter_speed = shutter_speed; - packet->target_system = target_system; - packet->target_component = target_component; - packet->mode = mode; - packet->aperture = aperture; - packet->iso = iso; - packet->exposure_type = exposure_type; - packet->command_id = command_id; - packet->engine_cut_off = engine_cut_off; - packet->extra_param = extra_param; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE DIGICAM_CONFIGURE UNPACKING - - -/** - * @brief Get field target_system from digicam_configure message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_digicam_configure_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field target_component from digicam_configure message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_digicam_configure_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field mode from digicam_configure message - * - * @return Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_configure_get_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field shutter_speed from digicam_configure message - * - * @return Divisor number //e.g. 1000 means 1/1000 (0 means ignore) - */ -static inline uint16_t mavlink_msg_digicam_configure_get_shutter_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field aperture from digicam_configure message - * - * @return F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_configure_get_aperture(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 9); -} - -/** - * @brief Get field iso from digicam_configure message - * - * @return ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_configure_get_iso(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field exposure_type from digicam_configure message - * - * @return Exposure type enumeration from 1 to N (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_configure_get_exposure_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field command_id from digicam_configure message - * - * @return Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - */ -static inline uint8_t mavlink_msg_digicam_configure_get_command_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field engine_cut_off from digicam_configure message - * - * @return Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) - */ -static inline uint8_t mavlink_msg_digicam_configure_get_engine_cut_off(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 13); -} - -/** - * @brief Get field extra_param from digicam_configure message - * - * @return Extra parameters enumeration (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_configure_get_extra_param(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field extra_value from digicam_configure message - * - * @return Correspondent value to given extra_param - */ -static inline float mavlink_msg_digicam_configure_get_extra_value(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a digicam_configure message into a struct - * - * @param msg The message to decode - * @param digicam_configure C-struct to decode the message contents into - */ -static inline void mavlink_msg_digicam_configure_decode(const mavlink_message_t* msg, mavlink_digicam_configure_t* digicam_configure) -{ -#if MAVLINK_NEED_BYTE_SWAP - digicam_configure->extra_value = mavlink_msg_digicam_configure_get_extra_value(msg); - digicam_configure->shutter_speed = mavlink_msg_digicam_configure_get_shutter_speed(msg); - digicam_configure->target_system = mavlink_msg_digicam_configure_get_target_system(msg); - digicam_configure->target_component = mavlink_msg_digicam_configure_get_target_component(msg); - digicam_configure->mode = mavlink_msg_digicam_configure_get_mode(msg); - digicam_configure->aperture = mavlink_msg_digicam_configure_get_aperture(msg); - digicam_configure->iso = mavlink_msg_digicam_configure_get_iso(msg); - digicam_configure->exposure_type = mavlink_msg_digicam_configure_get_exposure_type(msg); - digicam_configure->command_id = mavlink_msg_digicam_configure_get_command_id(msg); - digicam_configure->engine_cut_off = mavlink_msg_digicam_configure_get_engine_cut_off(msg); - digicam_configure->extra_param = mavlink_msg_digicam_configure_get_extra_param(msg); -#else - memcpy(digicam_configure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h deleted file mode 100644 index 68484f7b8..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h +++ /dev/null @@ -1,425 +0,0 @@ -// MESSAGE DIGICAM_CONTROL PACKING - -#define MAVLINK_MSG_ID_DIGICAM_CONTROL 155 - -typedef struct __mavlink_digicam_control_t -{ - float extra_value; ///< Correspondent value to given extra_param - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t session; ///< 0: stop, 1: start or keep it up //Session control e.g. show/hide lens - uint8_t zoom_pos; ///< 1 to N //Zoom's absolute position (0 means ignore) - int8_t zoom_step; ///< -100 to 100 //Zooming step value to offset zoom from the current position - uint8_t focus_lock; ///< 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus - uint8_t shot; ///< 0: ignore, 1: shot or start filming - uint8_t command_id; ///< Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - uint8_t extra_param; ///< Extra parameters enumeration (0 means ignore) -} mavlink_digicam_control_t; - -#define MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN 13 -#define MAVLINK_MSG_ID_155_LEN 13 - -#define MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC 22 -#define MAVLINK_MSG_ID_155_CRC 22 - - - -#define MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL { \ - "DIGICAM_CONTROL", \ - 10, \ - { { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_digicam_control_t, extra_value) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_digicam_control_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_digicam_control_t, target_component) }, \ - { "session", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_control_t, session) }, \ - { "zoom_pos", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_control_t, zoom_pos) }, \ - { "zoom_step", NULL, MAVLINK_TYPE_INT8_T, 0, 8, offsetof(mavlink_digicam_control_t, zoom_step) }, \ - { "focus_lock", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_digicam_control_t, focus_lock) }, \ - { "shot", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_digicam_control_t, shot) }, \ - { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_digicam_control_t, command_id) }, \ - { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_digicam_control_t, extra_param) }, \ - } \ -} - - -/** - * @brief Pack a digicam_control message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens - * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore) - * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position - * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus - * @param shot 0: ignore, 1: shot or start filming - * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - * @param extra_param Extra parameters enumeration (0 means ignore) - * @param extra_value Correspondent value to given extra_param - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_digicam_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN]; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 6, session); - _mav_put_uint8_t(buf, 7, zoom_pos); - _mav_put_int8_t(buf, 8, zoom_step); - _mav_put_uint8_t(buf, 9, focus_lock); - _mav_put_uint8_t(buf, 10, shot); - _mav_put_uint8_t(buf, 11, command_id); - _mav_put_uint8_t(buf, 12, extra_param); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#else - mavlink_digicam_control_t packet; - packet.extra_value = extra_value; - packet.target_system = target_system; - packet.target_component = target_component; - packet.session = session; - packet.zoom_pos = zoom_pos; - packet.zoom_step = zoom_step; - packet.focus_lock = focus_lock; - packet.shot = shot; - packet.command_id = command_id; - packet.extra_param = extra_param; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif -} - -/** - * @brief Pack a digicam_control message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens - * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore) - * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position - * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus - * @param shot 0: ignore, 1: shot or start filming - * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - * @param extra_param Extra parameters enumeration (0 means ignore) - * @param extra_value Correspondent value to given extra_param - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_digicam_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t session,uint8_t zoom_pos,int8_t zoom_step,uint8_t focus_lock,uint8_t shot,uint8_t command_id,uint8_t extra_param,float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN]; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 6, session); - _mav_put_uint8_t(buf, 7, zoom_pos); - _mav_put_int8_t(buf, 8, zoom_step); - _mav_put_uint8_t(buf, 9, focus_lock); - _mav_put_uint8_t(buf, 10, shot); - _mav_put_uint8_t(buf, 11, command_id); - _mav_put_uint8_t(buf, 12, extra_param); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#else - mavlink_digicam_control_t packet; - packet.extra_value = extra_value; - packet.target_system = target_system; - packet.target_component = target_component; - packet.session = session; - packet.zoom_pos = zoom_pos; - packet.zoom_step = zoom_step; - packet.focus_lock = focus_lock; - packet.shot = shot; - packet.command_id = command_id; - packet.extra_param = extra_param; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif -} - -/** - * @brief Encode a digicam_control struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param digicam_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_digicam_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control) -{ - return mavlink_msg_digicam_control_pack(system_id, component_id, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value); -} - -/** - * @brief Encode a digicam_control struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param digicam_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_digicam_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control) -{ - return mavlink_msg_digicam_control_pack_chan(system_id, component_id, chan, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value); -} - -/** - * @brief Send a digicam_control message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens - * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore) - * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position - * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus - * @param shot 0: ignore, 1: shot or start filming - * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - * @param extra_param Extra parameters enumeration (0 means ignore) - * @param extra_value Correspondent value to given extra_param - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_digicam_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN]; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 6, session); - _mav_put_uint8_t(buf, 7, zoom_pos); - _mav_put_int8_t(buf, 8, zoom_step); - _mav_put_uint8_t(buf, 9, focus_lock); - _mav_put_uint8_t(buf, 10, shot); - _mav_put_uint8_t(buf, 11, command_id); - _mav_put_uint8_t(buf, 12, extra_param); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif -#else - mavlink_digicam_control_t packet; - packet.extra_value = extra_value; - packet.target_system = target_system; - packet.target_component = target_component; - packet.session = session; - packet.zoom_pos = zoom_pos; - packet.zoom_step = zoom_step; - packet.focus_lock = focus_lock; - packet.shot = shot; - packet.command_id = command_id; - packet.extra_param = extra_param; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_digicam_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, extra_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 6, session); - _mav_put_uint8_t(buf, 7, zoom_pos); - _mav_put_int8_t(buf, 8, zoom_step); - _mav_put_uint8_t(buf, 9, focus_lock); - _mav_put_uint8_t(buf, 10, shot); - _mav_put_uint8_t(buf, 11, command_id); - _mav_put_uint8_t(buf, 12, extra_param); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif -#else - mavlink_digicam_control_t *packet = (mavlink_digicam_control_t *)msgbuf; - packet->extra_value = extra_value; - packet->target_system = target_system; - packet->target_component = target_component; - packet->session = session; - packet->zoom_pos = zoom_pos; - packet->zoom_step = zoom_step; - packet->focus_lock = focus_lock; - packet->shot = shot; - packet->command_id = command_id; - packet->extra_param = extra_param; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE DIGICAM_CONTROL UNPACKING - - -/** - * @brief Get field target_system from digicam_control message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_digicam_control_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field target_component from digicam_control message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_digicam_control_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field session from digicam_control message - * - * @return 0: stop, 1: start or keep it up //Session control e.g. show/hide lens - */ -static inline uint8_t mavlink_msg_digicam_control_get_session(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field zoom_pos from digicam_control message - * - * @return 1 to N //Zoom's absolute position (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_control_get_zoom_pos(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field zoom_step from digicam_control message - * - * @return -100 to 100 //Zooming step value to offset zoom from the current position - */ -static inline int8_t mavlink_msg_digicam_control_get_zoom_step(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int8_t(msg, 8); -} - -/** - * @brief Get field focus_lock from digicam_control message - * - * @return 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus - */ -static inline uint8_t mavlink_msg_digicam_control_get_focus_lock(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 9); -} - -/** - * @brief Get field shot from digicam_control message - * - * @return 0: ignore, 1: shot or start filming - */ -static inline uint8_t mavlink_msg_digicam_control_get_shot(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field command_id from digicam_control message - * - * @return Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once - */ -static inline uint8_t mavlink_msg_digicam_control_get_command_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field extra_param from digicam_control message - * - * @return Extra parameters enumeration (0 means ignore) - */ -static inline uint8_t mavlink_msg_digicam_control_get_extra_param(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field extra_value from digicam_control message - * - * @return Correspondent value to given extra_param - */ -static inline float mavlink_msg_digicam_control_get_extra_value(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a digicam_control message into a struct - * - * @param msg The message to decode - * @param digicam_control C-struct to decode the message contents into - */ -static inline void mavlink_msg_digicam_control_decode(const mavlink_message_t* msg, mavlink_digicam_control_t* digicam_control) -{ -#if MAVLINK_NEED_BYTE_SWAP - digicam_control->extra_value = mavlink_msg_digicam_control_get_extra_value(msg); - digicam_control->target_system = mavlink_msg_digicam_control_get_target_system(msg); - digicam_control->target_component = mavlink_msg_digicam_control_get_target_component(msg); - digicam_control->session = mavlink_msg_digicam_control_get_session(msg); - digicam_control->zoom_pos = mavlink_msg_digicam_control_get_zoom_pos(msg); - digicam_control->zoom_step = mavlink_msg_digicam_control_get_zoom_step(msg); - digicam_control->focus_lock = mavlink_msg_digicam_control_get_focus_lock(msg); - digicam_control->shot = mavlink_msg_digicam_control_get_shot(msg); - digicam_control->command_id = mavlink_msg_digicam_control_get_command_id(msg); - digicam_control->extra_param = mavlink_msg_digicam_control_get_extra_param(msg); -#else - memcpy(digicam_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h deleted file mode 100644 index 4021c7a09..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE FENCE_FETCH_POINT PACKING - -#define MAVLINK_MSG_ID_FENCE_FETCH_POINT 161 - -typedef struct __mavlink_fence_fetch_point_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t idx; ///< point index (first point is 1, 0 is for return point) -} mavlink_fence_fetch_point_t; - -#define MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN 3 -#define MAVLINK_MSG_ID_161_LEN 3 - -#define MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC 68 -#define MAVLINK_MSG_ID_161_CRC 68 - - - -#define MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT { \ - "FENCE_FETCH_POINT", \ - 3, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_fence_fetch_point_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_fence_fetch_point_t, target_component) }, \ - { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_fence_fetch_point_t, idx) }, \ - } \ -} - - -/** - * @brief Pack a fence_fetch_point message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 1, 0 is for return point) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_fence_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#else - mavlink_fence_fetch_point_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif -} - -/** - * @brief Pack a fence_fetch_point message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 1, 0 is for return point) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_fence_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#else - mavlink_fence_fetch_point_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif -} - -/** - * @brief Encode a fence_fetch_point struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param fence_fetch_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_fence_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point) -{ - return mavlink_msg_fence_fetch_point_pack(system_id, component_id, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx); -} - -/** - * @brief Encode a fence_fetch_point struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param fence_fetch_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_fence_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point) -{ - return mavlink_msg_fence_fetch_point_pack_chan(system_id, component_id, chan, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx); -} - -/** - * @brief Send a fence_fetch_point message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 1, 0 is for return point) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_fence_fetch_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif -#else - mavlink_fence_fetch_point_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_fence_fetch_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif -#else - mavlink_fence_fetch_point_t *packet = (mavlink_fence_fetch_point_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - packet->idx = idx; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FENCE_FETCH_POINT UNPACKING - - -/** - * @brief Get field target_system from fence_fetch_point message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_fence_fetch_point_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from fence_fetch_point message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_fence_fetch_point_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field idx from fence_fetch_point message - * - * @return point index (first point is 1, 0 is for return point) - */ -static inline uint8_t mavlink_msg_fence_fetch_point_get_idx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a fence_fetch_point message into a struct - * - * @param msg The message to decode - * @param fence_fetch_point C-struct to decode the message contents into - */ -static inline void mavlink_msg_fence_fetch_point_decode(const mavlink_message_t* msg, mavlink_fence_fetch_point_t* fence_fetch_point) -{ -#if MAVLINK_NEED_BYTE_SWAP - fence_fetch_point->target_system = mavlink_msg_fence_fetch_point_get_target_system(msg); - fence_fetch_point->target_component = mavlink_msg_fence_fetch_point_get_target_component(msg); - fence_fetch_point->idx = mavlink_msg_fence_fetch_point_get_idx(msg); -#else - memcpy(fence_fetch_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h deleted file mode 100644 index 6fc2c83fc..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE FENCE_POINT PACKING - -#define MAVLINK_MSG_ID_FENCE_POINT 160 - -typedef struct __mavlink_fence_point_t -{ - float lat; ///< Latitude of point - float lng; ///< Longitude of point - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t idx; ///< point index (first point is 1, 0 is for return point) - uint8_t count; ///< total number of points (for sanity checking) -} mavlink_fence_point_t; - -#define MAVLINK_MSG_ID_FENCE_POINT_LEN 12 -#define MAVLINK_MSG_ID_160_LEN 12 - -#define MAVLINK_MSG_ID_FENCE_POINT_CRC 78 -#define MAVLINK_MSG_ID_160_CRC 78 - - - -#define MAVLINK_MESSAGE_INFO_FENCE_POINT { \ - "FENCE_POINT", \ - 6, \ - { { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_fence_point_t, lat) }, \ - { "lng", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_fence_point_t, lng) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_fence_point_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_fence_point_t, target_component) }, \ - { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_fence_point_t, idx) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_fence_point_t, count) }, \ - } \ -} - - -/** - * @brief Pack a fence_point message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 1, 0 is for return point) - * @param count total number of points (for sanity checking) - * @param lat Latitude of point - * @param lng Longitude of point - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_fence_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_POINT_LEN]; - _mav_put_float(buf, 0, lat); - _mav_put_float(buf, 4, lng); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_uint8_t(buf, 10, idx); - _mav_put_uint8_t(buf, 11, count); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#else - mavlink_fence_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - packet.count = count; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCE_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif -} - -/** - * @brief Pack a fence_point message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 1, 0 is for return point) - * @param count total number of points (for sanity checking) - * @param lat Latitude of point - * @param lng Longitude of point - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_fence_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t idx,uint8_t count,float lat,float lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_POINT_LEN]; - _mav_put_float(buf, 0, lat); - _mav_put_float(buf, 4, lng); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_uint8_t(buf, 10, idx); - _mav_put_uint8_t(buf, 11, count); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#else - mavlink_fence_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - packet.count = count; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCE_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif -} - -/** - * @brief Encode a fence_point struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param fence_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_fence_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point) -{ - return mavlink_msg_fence_point_pack(system_id, component_id, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng); -} - -/** - * @brief Encode a fence_point struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param fence_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_fence_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point) -{ - return mavlink_msg_fence_point_pack_chan(system_id, component_id, chan, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng); -} - -/** - * @brief Send a fence_point message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 1, 0 is for return point) - * @param count total number of points (for sanity checking) - * @param lat Latitude of point - * @param lng Longitude of point - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_fence_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_POINT_LEN]; - _mav_put_float(buf, 0, lat); - _mav_put_float(buf, 4, lng); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_uint8_t(buf, 10, idx); - _mav_put_uint8_t(buf, 11, count); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif -#else - mavlink_fence_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - packet.count = count; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FENCE_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_fence_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, lat); - _mav_put_float(buf, 4, lng); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_uint8_t(buf, 10, idx); - _mav_put_uint8_t(buf, 11, count); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif -#else - mavlink_fence_point_t *packet = (mavlink_fence_point_t *)msgbuf; - packet->lat = lat; - packet->lng = lng; - packet->target_system = target_system; - packet->target_component = target_component; - packet->idx = idx; - packet->count = count; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FENCE_POINT UNPACKING - - -/** - * @brief Get field target_system from fence_point message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_fence_point_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field target_component from fence_point message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_fence_point_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 9); -} - -/** - * @brief Get field idx from fence_point message - * - * @return point index (first point is 1, 0 is for return point) - */ -static inline uint8_t mavlink_msg_fence_point_get_idx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field count from fence_point message - * - * @return total number of points (for sanity checking) - */ -static inline uint8_t mavlink_msg_fence_point_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field lat from fence_point message - * - * @return Latitude of point - */ -static inline float mavlink_msg_fence_point_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field lng from fence_point message - * - * @return Longitude of point - */ -static inline float mavlink_msg_fence_point_get_lng(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Decode a fence_point message into a struct - * - * @param msg The message to decode - * @param fence_point C-struct to decode the message contents into - */ -static inline void mavlink_msg_fence_point_decode(const mavlink_message_t* msg, mavlink_fence_point_t* fence_point) -{ -#if MAVLINK_NEED_BYTE_SWAP - fence_point->lat = mavlink_msg_fence_point_get_lat(msg); - fence_point->lng = mavlink_msg_fence_point_get_lng(msg); - fence_point->target_system = mavlink_msg_fence_point_get_target_system(msg); - fence_point->target_component = mavlink_msg_fence_point_get_target_component(msg); - fence_point->idx = mavlink_msg_fence_point_get_idx(msg); - fence_point->count = mavlink_msg_fence_point_get_count(msg); -#else - memcpy(fence_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_POINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h deleted file mode 100644 index c50d37724..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE FENCE_STATUS PACKING - -#define MAVLINK_MSG_ID_FENCE_STATUS 162 - -typedef struct __mavlink_fence_status_t -{ - uint32_t breach_time; ///< time of last breach in milliseconds since boot - uint16_t breach_count; ///< number of fence breaches - uint8_t breach_status; ///< 0 if currently inside fence, 1 if outside - uint8_t breach_type; ///< last breach type (see FENCE_BREACH_* enum) -} mavlink_fence_status_t; - -#define MAVLINK_MSG_ID_FENCE_STATUS_LEN 8 -#define MAVLINK_MSG_ID_162_LEN 8 - -#define MAVLINK_MSG_ID_FENCE_STATUS_CRC 189 -#define MAVLINK_MSG_ID_162_CRC 189 - - - -#define MAVLINK_MESSAGE_INFO_FENCE_STATUS { \ - "FENCE_STATUS", \ - 4, \ - { { "breach_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_fence_status_t, breach_time) }, \ - { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_fence_status_t, breach_count) }, \ - { "breach_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_fence_status_t, breach_status) }, \ - { "breach_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_fence_status_t, breach_type) }, \ - } \ -} - - -/** - * @brief Pack a fence_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param breach_status 0 if currently inside fence, 1 if outside - * @param breach_count number of fence breaches - * @param breach_type last breach type (see FENCE_BREACH_* enum) - * @param breach_time time of last breach in milliseconds since boot - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, breach_time); - _mav_put_uint16_t(buf, 4, breach_count); - _mav_put_uint8_t(buf, 6, breach_status); - _mav_put_uint8_t(buf, 7, breach_type); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#else - mavlink_fence_status_t packet; - packet.breach_time = breach_time; - packet.breach_count = breach_count; - packet.breach_status = breach_status; - packet.breach_type = breach_type; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif -} - -/** - * @brief Pack a fence_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param breach_status 0 if currently inside fence, 1 if outside - * @param breach_count number of fence breaches - * @param breach_type last breach type (see FENCE_BREACH_* enum) - * @param breach_time time of last breach in milliseconds since boot - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t breach_status,uint16_t breach_count,uint8_t breach_type,uint32_t breach_time) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, breach_time); - _mav_put_uint16_t(buf, 4, breach_count); - _mav_put_uint8_t(buf, 6, breach_status); - _mav_put_uint8_t(buf, 7, breach_type); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#else - mavlink_fence_status_t packet; - packet.breach_time = breach_time; - packet.breach_count = breach_count; - packet.breach_status = breach_status; - packet.breach_type = breach_type; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif -} - -/** - * @brief Encode a fence_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param fence_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) -{ - return mavlink_msg_fence_status_pack(system_id, component_id, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); -} - -/** - * @brief Encode a fence_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param fence_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_fence_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) -{ - return mavlink_msg_fence_status_pack_chan(system_id, component_id, chan, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); -} - -/** - * @brief Send a fence_status message - * @param chan MAVLink channel to send the message - * - * @param breach_status 0 if currently inside fence, 1 if outside - * @param breach_count number of fence breaches - * @param breach_type last breach type (see FENCE_BREACH_* enum) - * @param breach_time time of last breach in milliseconds since boot - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, breach_time); - _mav_put_uint16_t(buf, 4, breach_count); - _mav_put_uint8_t(buf, 6, breach_status); - _mav_put_uint8_t(buf, 7, breach_type); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif -#else - mavlink_fence_status_t packet; - packet.breach_time = breach_time; - packet.breach_count = breach_count; - packet.breach_status = breach_status; - packet.breach_type = breach_type; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FENCE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, breach_time); - _mav_put_uint16_t(buf, 4, breach_count); - _mav_put_uint8_t(buf, 6, breach_status); - _mav_put_uint8_t(buf, 7, breach_type); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif -#else - mavlink_fence_status_t *packet = (mavlink_fence_status_t *)msgbuf; - packet->breach_time = breach_time; - packet->breach_count = breach_count; - packet->breach_status = breach_status; - packet->breach_type = breach_type; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FENCE_STATUS UNPACKING - - -/** - * @brief Get field breach_status from fence_status message - * - * @return 0 if currently inside fence, 1 if outside - */ -static inline uint8_t mavlink_msg_fence_status_get_breach_status(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field breach_count from fence_status message - * - * @return number of fence breaches - */ -static inline uint16_t mavlink_msg_fence_status_get_breach_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field breach_type from fence_status message - * - * @return last breach type (see FENCE_BREACH_* enum) - */ -static inline uint8_t mavlink_msg_fence_status_get_breach_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field breach_time from fence_status message - * - * @return time of last breach in milliseconds since boot - */ -static inline uint32_t mavlink_msg_fence_status_get_breach_time(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Decode a fence_status message into a struct - * - * @param msg The message to decode - * @param fence_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_fence_status_decode(const mavlink_message_t* msg, mavlink_fence_status_t* fence_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - fence_status->breach_time = mavlink_msg_fence_status_get_breach_time(msg); - fence_status->breach_count = mavlink_msg_fence_status_get_breach_count(msg); - fence_status->breach_status = mavlink_msg_fence_status_get_breach_status(msg); - fence_status->breach_type = mavlink_msg_fence_status_get_breach_type(msg); -#else - memcpy(fence_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h deleted file mode 100644 index acf031f62..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE HWSTATUS PACKING - -#define MAVLINK_MSG_ID_HWSTATUS 165 - -typedef struct __mavlink_hwstatus_t -{ - uint16_t Vcc; ///< board voltage (mV) - uint8_t I2Cerr; ///< I2C error count -} mavlink_hwstatus_t; - -#define MAVLINK_MSG_ID_HWSTATUS_LEN 3 -#define MAVLINK_MSG_ID_165_LEN 3 - -#define MAVLINK_MSG_ID_HWSTATUS_CRC 21 -#define MAVLINK_MSG_ID_165_CRC 21 - - - -#define MAVLINK_MESSAGE_INFO_HWSTATUS { \ - "HWSTATUS", \ - 2, \ - { { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_hwstatus_t, Vcc) }, \ - { "I2Cerr", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_hwstatus_t, I2Cerr) }, \ - } \ -} - - -/** - * @brief Pack a hwstatus message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param Vcc board voltage (mV) - * @param I2Cerr I2C error count - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hwstatus_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t Vcc, uint8_t I2Cerr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HWSTATUS_LEN]; - _mav_put_uint16_t(buf, 0, Vcc); - _mav_put_uint8_t(buf, 2, I2Cerr); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HWSTATUS_LEN); -#else - mavlink_hwstatus_t packet; - packet.Vcc = Vcc; - packet.I2Cerr = I2Cerr; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HWSTATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif -} - -/** - * @brief Pack a hwstatus message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param Vcc board voltage (mV) - * @param I2Cerr I2C error count - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hwstatus_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t Vcc,uint8_t I2Cerr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HWSTATUS_LEN]; - _mav_put_uint16_t(buf, 0, Vcc); - _mav_put_uint8_t(buf, 2, I2Cerr); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HWSTATUS_LEN); -#else - mavlink_hwstatus_t packet; - packet.Vcc = Vcc; - packet.I2Cerr = I2Cerr; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HWSTATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif -} - -/** - * @brief Encode a hwstatus struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param hwstatus C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hwstatus_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus) -{ - return mavlink_msg_hwstatus_pack(system_id, component_id, msg, hwstatus->Vcc, hwstatus->I2Cerr); -} - -/** - * @brief Encode a hwstatus struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param hwstatus C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hwstatus_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus) -{ - return mavlink_msg_hwstatus_pack_chan(system_id, component_id, chan, msg, hwstatus->Vcc, hwstatus->I2Cerr); -} - -/** - * @brief Send a hwstatus message - * @param chan MAVLink channel to send the message - * - * @param Vcc board voltage (mV) - * @param I2Cerr I2C error count - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_hwstatus_send(mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HWSTATUS_LEN]; - _mav_put_uint16_t(buf, 0, Vcc); - _mav_put_uint8_t(buf, 2, I2Cerr); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif -#else - mavlink_hwstatus_t packet; - packet.Vcc = Vcc; - packet.I2Cerr = I2Cerr; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_HWSTATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_hwstatus_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, Vcc); - _mav_put_uint8_t(buf, 2, I2Cerr); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif -#else - mavlink_hwstatus_t *packet = (mavlink_hwstatus_t *)msgbuf; - packet->Vcc = Vcc; - packet->I2Cerr = I2Cerr; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)packet, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)packet, MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE HWSTATUS UNPACKING - - -/** - * @brief Get field Vcc from hwstatus message - * - * @return board voltage (mV) - */ -static inline uint16_t mavlink_msg_hwstatus_get_Vcc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field I2Cerr from hwstatus message - * - * @return I2C error count - */ -static inline uint8_t mavlink_msg_hwstatus_get_I2Cerr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a hwstatus message into a struct - * - * @param msg The message to decode - * @param hwstatus C-struct to decode the message contents into - */ -static inline void mavlink_msg_hwstatus_decode(const mavlink_message_t* msg, mavlink_hwstatus_t* hwstatus) -{ -#if MAVLINK_NEED_BYTE_SWAP - hwstatus->Vcc = mavlink_msg_hwstatus_get_Vcc(msg); - hwstatus->I2Cerr = mavlink_msg_hwstatus_get_I2Cerr(msg); -#else - memcpy(hwstatus, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HWSTATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h deleted file mode 100644 index 5fef93718..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h +++ /dev/null @@ -1,401 +0,0 @@ -// MESSAGE LIMITS_STATUS PACKING - -#define MAVLINK_MSG_ID_LIMITS_STATUS 167 - -typedef struct __mavlink_limits_status_t -{ - uint32_t last_trigger; ///< time of last breach in milliseconds since boot - uint32_t last_action; ///< time of last recovery action in milliseconds since boot - uint32_t last_recovery; ///< time of last successful recovery in milliseconds since boot - uint32_t last_clear; ///< time of last all-clear in milliseconds since boot - uint16_t breach_count; ///< number of fence breaches - uint8_t limits_state; ///< state of AP_Limits, (see enum LimitState, LIMITS_STATE) - uint8_t mods_enabled; ///< AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) - uint8_t mods_required; ///< AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) - uint8_t mods_triggered; ///< AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) -} mavlink_limits_status_t; - -#define MAVLINK_MSG_ID_LIMITS_STATUS_LEN 22 -#define MAVLINK_MSG_ID_167_LEN 22 - -#define MAVLINK_MSG_ID_LIMITS_STATUS_CRC 144 -#define MAVLINK_MSG_ID_167_CRC 144 - - - -#define MAVLINK_MESSAGE_INFO_LIMITS_STATUS { \ - "LIMITS_STATUS", \ - 9, \ - { { "last_trigger", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_limits_status_t, last_trigger) }, \ - { "last_action", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_limits_status_t, last_action) }, \ - { "last_recovery", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_limits_status_t, last_recovery) }, \ - { "last_clear", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_limits_status_t, last_clear) }, \ - { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_limits_status_t, breach_count) }, \ - { "limits_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_limits_status_t, limits_state) }, \ - { "mods_enabled", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_limits_status_t, mods_enabled) }, \ - { "mods_required", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_limits_status_t, mods_required) }, \ - { "mods_triggered", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_limits_status_t, mods_triggered) }, \ - } \ -} - - -/** - * @brief Pack a limits_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) - * @param last_trigger time of last breach in milliseconds since boot - * @param last_action time of last recovery action in milliseconds since boot - * @param last_recovery time of last successful recovery in milliseconds since boot - * @param last_clear time of last all-clear in milliseconds since boot - * @param breach_count number of fence breaches - * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) - * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) - * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LIMITS_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, last_trigger); - _mav_put_uint32_t(buf, 4, last_action); - _mav_put_uint32_t(buf, 8, last_recovery); - _mav_put_uint32_t(buf, 12, last_clear); - _mav_put_uint16_t(buf, 16, breach_count); - _mav_put_uint8_t(buf, 18, limits_state); - _mav_put_uint8_t(buf, 19, mods_enabled); - _mav_put_uint8_t(buf, 20, mods_required); - _mav_put_uint8_t(buf, 21, mods_triggered); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#else - mavlink_limits_status_t packet; - packet.last_trigger = last_trigger; - packet.last_action = last_action; - packet.last_recovery = last_recovery; - packet.last_clear = last_clear; - packet.breach_count = breach_count; - packet.limits_state = limits_state; - packet.mods_enabled = mods_enabled; - packet.mods_required = mods_required; - packet.mods_triggered = mods_triggered; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif -} - -/** - * @brief Pack a limits_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) - * @param last_trigger time of last breach in milliseconds since boot - * @param last_action time of last recovery action in milliseconds since boot - * @param last_recovery time of last successful recovery in milliseconds since boot - * @param last_clear time of last all-clear in milliseconds since boot - * @param breach_count number of fence breaches - * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) - * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) - * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t limits_state,uint32_t last_trigger,uint32_t last_action,uint32_t last_recovery,uint32_t last_clear,uint16_t breach_count,uint8_t mods_enabled,uint8_t mods_required,uint8_t mods_triggered) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LIMITS_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, last_trigger); - _mav_put_uint32_t(buf, 4, last_action); - _mav_put_uint32_t(buf, 8, last_recovery); - _mav_put_uint32_t(buf, 12, last_clear); - _mav_put_uint16_t(buf, 16, breach_count); - _mav_put_uint8_t(buf, 18, limits_state); - _mav_put_uint8_t(buf, 19, mods_enabled); - _mav_put_uint8_t(buf, 20, mods_required); - _mav_put_uint8_t(buf, 21, mods_triggered); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#else - mavlink_limits_status_t packet; - packet.last_trigger = last_trigger; - packet.last_action = last_action; - packet.last_recovery = last_recovery; - packet.last_clear = last_clear; - packet.breach_count = breach_count; - packet.limits_state = limits_state; - packet.mods_enabled = mods_enabled; - packet.mods_required = mods_required; - packet.mods_triggered = mods_triggered; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif -} - -/** - * @brief Encode a limits_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param limits_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status) -{ - return mavlink_msg_limits_status_pack(system_id, component_id, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered); -} - -/** - * @brief Encode a limits_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param limits_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_limits_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status) -{ - return mavlink_msg_limits_status_pack_chan(system_id, component_id, chan, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered); -} - -/** - * @brief Send a limits_status message - * @param chan MAVLink channel to send the message - * - * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) - * @param last_trigger time of last breach in milliseconds since boot - * @param last_action time of last recovery action in milliseconds since boot - * @param last_recovery time of last successful recovery in milliseconds since boot - * @param last_clear time of last all-clear in milliseconds since boot - * @param breach_count number of fence breaches - * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) - * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) - * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_limits_status_send(mavlink_channel_t chan, uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LIMITS_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, last_trigger); - _mav_put_uint32_t(buf, 4, last_action); - _mav_put_uint32_t(buf, 8, last_recovery); - _mav_put_uint32_t(buf, 12, last_clear); - _mav_put_uint16_t(buf, 16, breach_count); - _mav_put_uint8_t(buf, 18, limits_state); - _mav_put_uint8_t(buf, 19, mods_enabled); - _mav_put_uint8_t(buf, 20, mods_required); - _mav_put_uint8_t(buf, 21, mods_triggered); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif -#else - mavlink_limits_status_t packet; - packet.last_trigger = last_trigger; - packet.last_action = last_action; - packet.last_recovery = last_recovery; - packet.last_clear = last_clear; - packet.breach_count = breach_count; - packet.limits_state = limits_state; - packet.mods_enabled = mods_enabled; - packet.mods_required = mods_required; - packet.mods_triggered = mods_triggered; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_LIMITS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_limits_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, last_trigger); - _mav_put_uint32_t(buf, 4, last_action); - _mav_put_uint32_t(buf, 8, last_recovery); - _mav_put_uint32_t(buf, 12, last_clear); - _mav_put_uint16_t(buf, 16, breach_count); - _mav_put_uint8_t(buf, 18, limits_state); - _mav_put_uint8_t(buf, 19, mods_enabled); - _mav_put_uint8_t(buf, 20, mods_required); - _mav_put_uint8_t(buf, 21, mods_triggered); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif -#else - mavlink_limits_status_t *packet = (mavlink_limits_status_t *)msgbuf; - packet->last_trigger = last_trigger; - packet->last_action = last_action; - packet->last_recovery = last_recovery; - packet->last_clear = last_clear; - packet->breach_count = breach_count; - packet->limits_state = limits_state; - packet->mods_enabled = mods_enabled; - packet->mods_required = mods_required; - packet->mods_triggered = mods_triggered; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE LIMITS_STATUS UNPACKING - - -/** - * @brief Get field limits_state from limits_status message - * - * @return state of AP_Limits, (see enum LimitState, LIMITS_STATE) - */ -static inline uint8_t mavlink_msg_limits_status_get_limits_state(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 18); -} - -/** - * @brief Get field last_trigger from limits_status message - * - * @return time of last breach in milliseconds since boot - */ -static inline uint32_t mavlink_msg_limits_status_get_last_trigger(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field last_action from limits_status message - * - * @return time of last recovery action in milliseconds since boot - */ -static inline uint32_t mavlink_msg_limits_status_get_last_action(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 4); -} - -/** - * @brief Get field last_recovery from limits_status message - * - * @return time of last successful recovery in milliseconds since boot - */ -static inline uint32_t mavlink_msg_limits_status_get_last_recovery(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 8); -} - -/** - * @brief Get field last_clear from limits_status message - * - * @return time of last all-clear in milliseconds since boot - */ -static inline uint32_t mavlink_msg_limits_status_get_last_clear(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 12); -} - -/** - * @brief Get field breach_count from limits_status message - * - * @return number of fence breaches - */ -static inline uint16_t mavlink_msg_limits_status_get_breach_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field mods_enabled from limits_status message - * - * @return AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) - */ -static inline uint8_t mavlink_msg_limits_status_get_mods_enabled(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 19); -} - -/** - * @brief Get field mods_required from limits_status message - * - * @return AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) - */ -static inline uint8_t mavlink_msg_limits_status_get_mods_required(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 20); -} - -/** - * @brief Get field mods_triggered from limits_status message - * - * @return AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) - */ -static inline uint8_t mavlink_msg_limits_status_get_mods_triggered(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 21); -} - -/** - * @brief Decode a limits_status message into a struct - * - * @param msg The message to decode - * @param limits_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_limits_status_decode(const mavlink_message_t* msg, mavlink_limits_status_t* limits_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - limits_status->last_trigger = mavlink_msg_limits_status_get_last_trigger(msg); - limits_status->last_action = mavlink_msg_limits_status_get_last_action(msg); - limits_status->last_recovery = mavlink_msg_limits_status_get_last_recovery(msg); - limits_status->last_clear = mavlink_msg_limits_status_get_last_clear(msg); - limits_status->breach_count = mavlink_msg_limits_status_get_breach_count(msg); - limits_status->limits_state = mavlink_msg_limits_status_get_limits_state(msg); - limits_status->mods_enabled = mavlink_msg_limits_status_get_mods_enabled(msg); - limits_status->mods_required = mavlink_msg_limits_status_get_mods_required(msg); - limits_status->mods_triggered = mavlink_msg_limits_status_get_mods_triggered(msg); -#else - memcpy(limits_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LIMITS_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h deleted file mode 100644 index c64b2e937..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE MEMINFO PACKING - -#define MAVLINK_MSG_ID_MEMINFO 152 - -typedef struct __mavlink_meminfo_t -{ - uint16_t brkval; ///< heap top - uint16_t freemem; ///< free memory -} mavlink_meminfo_t; - -#define MAVLINK_MSG_ID_MEMINFO_LEN 4 -#define MAVLINK_MSG_ID_152_LEN 4 - -#define MAVLINK_MSG_ID_MEMINFO_CRC 208 -#define MAVLINK_MSG_ID_152_CRC 208 - - - -#define MAVLINK_MESSAGE_INFO_MEMINFO { \ - "MEMINFO", \ - 2, \ - { { "brkval", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_meminfo_t, brkval) }, \ - { "freemem", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_meminfo_t, freemem) }, \ - } \ -} - - -/** - * @brief Pack a meminfo message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param brkval heap top - * @param freemem free memory - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_meminfo_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t brkval, uint16_t freemem) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MEMINFO_LEN]; - _mav_put_uint16_t(buf, 0, brkval); - _mav_put_uint16_t(buf, 2, freemem); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMINFO_LEN); -#else - mavlink_meminfo_t packet; - packet.brkval = brkval; - packet.freemem = freemem; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MEMINFO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif -} - -/** - * @brief Pack a meminfo message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param brkval heap top - * @param freemem free memory - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_meminfo_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t brkval,uint16_t freemem) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MEMINFO_LEN]; - _mav_put_uint16_t(buf, 0, brkval); - _mav_put_uint16_t(buf, 2, freemem); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMINFO_LEN); -#else - mavlink_meminfo_t packet; - packet.brkval = brkval; - packet.freemem = freemem; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MEMINFO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif -} - -/** - * @brief Encode a meminfo struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param meminfo C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_meminfo_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_meminfo_t* meminfo) -{ - return mavlink_msg_meminfo_pack(system_id, component_id, msg, meminfo->brkval, meminfo->freemem); -} - -/** - * @brief Encode a meminfo struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param meminfo C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_meminfo_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_meminfo_t* meminfo) -{ - return mavlink_msg_meminfo_pack_chan(system_id, component_id, chan, msg, meminfo->brkval, meminfo->freemem); -} - -/** - * @brief Send a meminfo message - * @param chan MAVLink channel to send the message - * - * @param brkval heap top - * @param freemem free memory - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_meminfo_send(mavlink_channel_t chan, uint16_t brkval, uint16_t freemem) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MEMINFO_LEN]; - _mav_put_uint16_t(buf, 0, brkval); - _mav_put_uint16_t(buf, 2, freemem); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif -#else - mavlink_meminfo_t packet; - packet.brkval = brkval; - packet.freemem = freemem; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)&packet, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)&packet, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MEMINFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_meminfo_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t brkval, uint16_t freemem) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, brkval); - _mav_put_uint16_t(buf, 2, freemem); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif -#else - mavlink_meminfo_t *packet = (mavlink_meminfo_t *)msgbuf; - packet->brkval = brkval; - packet->freemem = freemem; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)packet, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)packet, MAVLINK_MSG_ID_MEMINFO_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MEMINFO UNPACKING - - -/** - * @brief Get field brkval from meminfo message - * - * @return heap top - */ -static inline uint16_t mavlink_msg_meminfo_get_brkval(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field freemem from meminfo message - * - * @return free memory - */ -static inline uint16_t mavlink_msg_meminfo_get_freemem(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Decode a meminfo message into a struct - * - * @param msg The message to decode - * @param meminfo C-struct to decode the message contents into - */ -static inline void mavlink_msg_meminfo_decode(const mavlink_message_t* msg, mavlink_meminfo_t* meminfo) -{ -#if MAVLINK_NEED_BYTE_SWAP - meminfo->brkval = mavlink_msg_meminfo_get_brkval(msg); - meminfo->freemem = mavlink_msg_meminfo_get_freemem(msg); -#else - memcpy(meminfo, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MEMINFO_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h deleted file mode 100644 index 350bb0b78..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE MOUNT_CONFIGURE PACKING - -#define MAVLINK_MSG_ID_MOUNT_CONFIGURE 156 - -typedef struct __mavlink_mount_configure_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t mount_mode; ///< mount operating mode (see MAV_MOUNT_MODE enum) - uint8_t stab_roll; ///< (1 = yes, 0 = no) - uint8_t stab_pitch; ///< (1 = yes, 0 = no) - uint8_t stab_yaw; ///< (1 = yes, 0 = no) -} mavlink_mount_configure_t; - -#define MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN 6 -#define MAVLINK_MSG_ID_156_LEN 6 - -#define MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC 19 -#define MAVLINK_MSG_ID_156_CRC 19 - - - -#define MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE { \ - "MOUNT_CONFIGURE", \ - 6, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mount_configure_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mount_configure_t, target_component) }, \ - { "mount_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mount_configure_t, mount_mode) }, \ - { "stab_roll", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mount_configure_t, stab_roll) }, \ - { "stab_pitch", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mount_configure_t, stab_pitch) }, \ - { "stab_yaw", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mount_configure_t, stab_yaw) }, \ - } \ -} - - -/** - * @brief Pack a mount_configure message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum) - * @param stab_roll (1 = yes, 0 = no) - * @param stab_pitch (1 = yes, 0 = no) - * @param stab_yaw (1 = yes, 0 = no) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mount_configure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, mount_mode); - _mav_put_uint8_t(buf, 3, stab_roll); - _mav_put_uint8_t(buf, 4, stab_pitch); - _mav_put_uint8_t(buf, 5, stab_yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#else - mavlink_mount_configure_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.mount_mode = mount_mode; - packet.stab_roll = stab_roll; - packet.stab_pitch = stab_pitch; - packet.stab_yaw = stab_yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif -} - -/** - * @brief Pack a mount_configure message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum) - * @param stab_roll (1 = yes, 0 = no) - * @param stab_pitch (1 = yes, 0 = no) - * @param stab_yaw (1 = yes, 0 = no) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mount_configure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t mount_mode,uint8_t stab_roll,uint8_t stab_pitch,uint8_t stab_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, mount_mode); - _mav_put_uint8_t(buf, 3, stab_roll); - _mav_put_uint8_t(buf, 4, stab_pitch); - _mav_put_uint8_t(buf, 5, stab_yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#else - mavlink_mount_configure_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.mount_mode = mount_mode; - packet.stab_roll = stab_roll; - packet.stab_pitch = stab_pitch; - packet.stab_yaw = stab_yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif -} - -/** - * @brief Encode a mount_configure struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mount_configure C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mount_configure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_configure_t* mount_configure) -{ - return mavlink_msg_mount_configure_pack(system_id, component_id, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw); -} - -/** - * @brief Encode a mount_configure struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mount_configure C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mount_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_configure_t* mount_configure) -{ - return mavlink_msg_mount_configure_pack_chan(system_id, component_id, chan, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw); -} - -/** - * @brief Send a mount_configure message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum) - * @param stab_roll (1 = yes, 0 = no) - * @param stab_pitch (1 = yes, 0 = no) - * @param stab_yaw (1 = yes, 0 = no) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mount_configure_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, mount_mode); - _mav_put_uint8_t(buf, 3, stab_roll); - _mav_put_uint8_t(buf, 4, stab_pitch); - _mav_put_uint8_t(buf, 5, stab_yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif -#else - mavlink_mount_configure_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.mount_mode = mount_mode; - packet.stab_roll = stab_roll; - packet.stab_pitch = stab_pitch; - packet.stab_yaw = stab_yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mount_configure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, mount_mode); - _mav_put_uint8_t(buf, 3, stab_roll); - _mav_put_uint8_t(buf, 4, stab_pitch); - _mav_put_uint8_t(buf, 5, stab_yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif -#else - mavlink_mount_configure_t *packet = (mavlink_mount_configure_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - packet->mount_mode = mount_mode; - packet->stab_roll = stab_roll; - packet->stab_pitch = stab_pitch; - packet->stab_yaw = stab_yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MOUNT_CONFIGURE UNPACKING - - -/** - * @brief Get field target_system from mount_configure message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mount_configure_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from mount_configure message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mount_configure_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field mount_mode from mount_configure message - * - * @return mount operating mode (see MAV_MOUNT_MODE enum) - */ -static inline uint8_t mavlink_msg_mount_configure_get_mount_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field stab_roll from mount_configure message - * - * @return (1 = yes, 0 = no) - */ -static inline uint8_t mavlink_msg_mount_configure_get_stab_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field stab_pitch from mount_configure message - * - * @return (1 = yes, 0 = no) - */ -static inline uint8_t mavlink_msg_mount_configure_get_stab_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field stab_yaw from mount_configure message - * - * @return (1 = yes, 0 = no) - */ -static inline uint8_t mavlink_msg_mount_configure_get_stab_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Decode a mount_configure message into a struct - * - * @param msg The message to decode - * @param mount_configure C-struct to decode the message contents into - */ -static inline void mavlink_msg_mount_configure_decode(const mavlink_message_t* msg, mavlink_mount_configure_t* mount_configure) -{ -#if MAVLINK_NEED_BYTE_SWAP - mount_configure->target_system = mavlink_msg_mount_configure_get_target_system(msg); - mount_configure->target_component = mavlink_msg_mount_configure_get_target_component(msg); - mount_configure->mount_mode = mavlink_msg_mount_configure_get_mount_mode(msg); - mount_configure->stab_roll = mavlink_msg_mount_configure_get_stab_roll(msg); - mount_configure->stab_pitch = mavlink_msg_mount_configure_get_stab_pitch(msg); - mount_configure->stab_yaw = mavlink_msg_mount_configure_get_stab_yaw(msg); -#else - memcpy(mount_configure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h deleted file mode 100644 index 72d6e2419..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE MOUNT_CONTROL PACKING - -#define MAVLINK_MSG_ID_MOUNT_CONTROL 157 - -typedef struct __mavlink_mount_control_t -{ - int32_t input_a; ///< pitch(deg*100) or lat, depending on mount mode - int32_t input_b; ///< roll(deg*100) or lon depending on mount mode - int32_t input_c; ///< yaw(deg*100) or alt (in cm) depending on mount mode - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t save_position; ///< if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) -} mavlink_mount_control_t; - -#define MAVLINK_MSG_ID_MOUNT_CONTROL_LEN 15 -#define MAVLINK_MSG_ID_157_LEN 15 - -#define MAVLINK_MSG_ID_MOUNT_CONTROL_CRC 21 -#define MAVLINK_MSG_ID_157_CRC 21 - - - -#define MAVLINK_MESSAGE_INFO_MOUNT_CONTROL { \ - "MOUNT_CONTROL", \ - 6, \ - { { "input_a", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_mount_control_t, input_a) }, \ - { "input_b", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_mount_control_t, input_b) }, \ - { "input_c", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_mount_control_t, input_c) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mount_control_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mount_control_t, target_component) }, \ - { "save_position", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_mount_control_t, save_position) }, \ - } \ -} - - -/** - * @brief Pack a mount_control message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param input_a pitch(deg*100) or lat, depending on mount mode - * @param input_b roll(deg*100) or lon depending on mount mode - * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode - * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mount_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN]; - _mav_put_int32_t(buf, 0, input_a); - _mav_put_int32_t(buf, 4, input_b); - _mav_put_int32_t(buf, 8, input_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - _mav_put_uint8_t(buf, 14, save_position); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#else - mavlink_mount_control_t packet; - packet.input_a = input_a; - packet.input_b = input_b; - packet.input_c = input_c; - packet.target_system = target_system; - packet.target_component = target_component; - packet.save_position = save_position; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif -} - -/** - * @brief Pack a mount_control message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param input_a pitch(deg*100) or lat, depending on mount mode - * @param input_b roll(deg*100) or lon depending on mount mode - * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode - * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mount_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int32_t input_a,int32_t input_b,int32_t input_c,uint8_t save_position) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN]; - _mav_put_int32_t(buf, 0, input_a); - _mav_put_int32_t(buf, 4, input_b); - _mav_put_int32_t(buf, 8, input_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - _mav_put_uint8_t(buf, 14, save_position); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#else - mavlink_mount_control_t packet; - packet.input_a = input_a; - packet.input_b = input_b; - packet.input_c = input_c; - packet.target_system = target_system; - packet.target_component = target_component; - packet.save_position = save_position; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif -} - -/** - * @brief Encode a mount_control struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mount_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mount_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_control_t* mount_control) -{ - return mavlink_msg_mount_control_pack(system_id, component_id, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position); -} - -/** - * @brief Encode a mount_control struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mount_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mount_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_control_t* mount_control) -{ - return mavlink_msg_mount_control_pack_chan(system_id, component_id, chan, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position); -} - -/** - * @brief Send a mount_control message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param input_a pitch(deg*100) or lat, depending on mount mode - * @param input_b roll(deg*100) or lon depending on mount mode - * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode - * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mount_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN]; - _mav_put_int32_t(buf, 0, input_a); - _mav_put_int32_t(buf, 4, input_b); - _mav_put_int32_t(buf, 8, input_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - _mav_put_uint8_t(buf, 14, save_position); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif -#else - mavlink_mount_control_t packet; - packet.input_a = input_a; - packet.input_b = input_b; - packet.input_c = input_c; - packet.target_system = target_system; - packet.target_component = target_component; - packet.save_position = save_position; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MOUNT_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mount_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, input_a); - _mav_put_int32_t(buf, 4, input_b); - _mav_put_int32_t(buf, 8, input_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - _mav_put_uint8_t(buf, 14, save_position); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif -#else - mavlink_mount_control_t *packet = (mavlink_mount_control_t *)msgbuf; - packet->input_a = input_a; - packet->input_b = input_b; - packet->input_c = input_c; - packet->target_system = target_system; - packet->target_component = target_component; - packet->save_position = save_position; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MOUNT_CONTROL UNPACKING - - -/** - * @brief Get field target_system from mount_control message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mount_control_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field target_component from mount_control message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mount_control_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 13); -} - -/** - * @brief Get field input_a from mount_control message - * - * @return pitch(deg*100) or lat, depending on mount mode - */ -static inline int32_t mavlink_msg_mount_control_get_input_a(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field input_b from mount_control message - * - * @return roll(deg*100) or lon depending on mount mode - */ -static inline int32_t mavlink_msg_mount_control_get_input_b(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field input_c from mount_control message - * - * @return yaw(deg*100) or alt (in cm) depending on mount mode - */ -static inline int32_t mavlink_msg_mount_control_get_input_c(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field save_position from mount_control message - * - * @return if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) - */ -static inline uint8_t mavlink_msg_mount_control_get_save_position(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Decode a mount_control message into a struct - * - * @param msg The message to decode - * @param mount_control C-struct to decode the message contents into - */ -static inline void mavlink_msg_mount_control_decode(const mavlink_message_t* msg, mavlink_mount_control_t* mount_control) -{ -#if MAVLINK_NEED_BYTE_SWAP - mount_control->input_a = mavlink_msg_mount_control_get_input_a(msg); - mount_control->input_b = mavlink_msg_mount_control_get_input_b(msg); - mount_control->input_c = mavlink_msg_mount_control_get_input_c(msg); - mount_control->target_system = mavlink_msg_mount_control_get_target_system(msg); - mount_control->target_component = mavlink_msg_mount_control_get_target_component(msg); - mount_control->save_position = mavlink_msg_mount_control_get_save_position(msg); -#else - memcpy(mount_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h deleted file mode 100644 index c140dd2c7..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE MOUNT_STATUS PACKING - -#define MAVLINK_MSG_ID_MOUNT_STATUS 158 - -typedef struct __mavlink_mount_status_t -{ - int32_t pointing_a; ///< pitch(deg*100) or lat, depending on mount mode - int32_t pointing_b; ///< roll(deg*100) or lon depending on mount mode - int32_t pointing_c; ///< yaw(deg*100) or alt (in cm) depending on mount mode - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_mount_status_t; - -#define MAVLINK_MSG_ID_MOUNT_STATUS_LEN 14 -#define MAVLINK_MSG_ID_158_LEN 14 - -#define MAVLINK_MSG_ID_MOUNT_STATUS_CRC 134 -#define MAVLINK_MSG_ID_158_CRC 134 - - - -#define MAVLINK_MESSAGE_INFO_MOUNT_STATUS { \ - "MOUNT_STATUS", \ - 5, \ - { { "pointing_a", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_mount_status_t, pointing_a) }, \ - { "pointing_b", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_mount_status_t, pointing_b) }, \ - { "pointing_c", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_mount_status_t, pointing_c) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mount_status_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mount_status_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a mount_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param pointing_a pitch(deg*100) or lat, depending on mount mode - * @param pointing_b roll(deg*100) or lon depending on mount mode - * @param pointing_c yaw(deg*100) or alt (in cm) depending on mount mode - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mount_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_STATUS_LEN]; - _mav_put_int32_t(buf, 0, pointing_a); - _mav_put_int32_t(buf, 4, pointing_b); - _mav_put_int32_t(buf, 8, pointing_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#else - mavlink_mount_status_t packet; - packet.pointing_a = pointing_a; - packet.pointing_b = pointing_b; - packet.pointing_c = pointing_c; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif -} - -/** - * @brief Pack a mount_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param pointing_a pitch(deg*100) or lat, depending on mount mode - * @param pointing_b roll(deg*100) or lon depending on mount mode - * @param pointing_c yaw(deg*100) or alt (in cm) depending on mount mode - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mount_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int32_t pointing_a,int32_t pointing_b,int32_t pointing_c) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_STATUS_LEN]; - _mav_put_int32_t(buf, 0, pointing_a); - _mav_put_int32_t(buf, 4, pointing_b); - _mav_put_int32_t(buf, 8, pointing_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#else - mavlink_mount_status_t packet; - packet.pointing_a = pointing_a; - packet.pointing_b = pointing_b; - packet.pointing_c = pointing_c; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif -} - -/** - * @brief Encode a mount_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mount_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mount_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_status_t* mount_status) -{ - return mavlink_msg_mount_status_pack(system_id, component_id, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c); -} - -/** - * @brief Encode a mount_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mount_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mount_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_status_t* mount_status) -{ - return mavlink_msg_mount_status_pack_chan(system_id, component_id, chan, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c); -} - -/** - * @brief Send a mount_status message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param pointing_a pitch(deg*100) or lat, depending on mount mode - * @param pointing_b roll(deg*100) or lon depending on mount mode - * @param pointing_c yaw(deg*100) or alt (in cm) depending on mount mode - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mount_status_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MOUNT_STATUS_LEN]; - _mav_put_int32_t(buf, 0, pointing_a); - _mav_put_int32_t(buf, 4, pointing_b); - _mav_put_int32_t(buf, 8, pointing_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif -#else - mavlink_mount_status_t packet; - packet.pointing_a = pointing_a; - packet.pointing_b = pointing_b; - packet.pointing_c = pointing_c; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MOUNT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mount_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, pointing_a); - _mav_put_int32_t(buf, 4, pointing_b); - _mav_put_int32_t(buf, 8, pointing_c); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif -#else - mavlink_mount_status_t *packet = (mavlink_mount_status_t *)msgbuf; - packet->pointing_a = pointing_a; - packet->pointing_b = pointing_b; - packet->pointing_c = pointing_c; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MOUNT_STATUS UNPACKING - - -/** - * @brief Get field target_system from mount_status message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mount_status_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field target_component from mount_status message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mount_status_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 13); -} - -/** - * @brief Get field pointing_a from mount_status message - * - * @return pitch(deg*100) or lat, depending on mount mode - */ -static inline int32_t mavlink_msg_mount_status_get_pointing_a(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field pointing_b from mount_status message - * - * @return roll(deg*100) or lon depending on mount mode - */ -static inline int32_t mavlink_msg_mount_status_get_pointing_b(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field pointing_c from mount_status message - * - * @return yaw(deg*100) or alt (in cm) depending on mount mode - */ -static inline int32_t mavlink_msg_mount_status_get_pointing_c(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Decode a mount_status message into a struct - * - * @param msg The message to decode - * @param mount_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_mount_status_decode(const mavlink_message_t* msg, mavlink_mount_status_t* mount_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - mount_status->pointing_a = mavlink_msg_mount_status_get_pointing_a(msg); - mount_status->pointing_b = mavlink_msg_mount_status_get_pointing_b(msg); - mount_status->pointing_c = mavlink_msg_mount_status_get_pointing_c(msg); - mount_status->target_system = mavlink_msg_mount_status_get_target_system(msg); - mount_status->target_component = mavlink_msg_mount_status_get_target_component(msg); -#else - memcpy(mount_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h deleted file mode 100644 index 830eb4639..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE RADIO PACKING - -#define MAVLINK_MSG_ID_RADIO 166 - -typedef struct __mavlink_radio_t -{ - uint16_t rxerrors; ///< receive errors - uint16_t fixed; ///< count of error corrected packets - uint8_t rssi; ///< local signal strength - uint8_t remrssi; ///< remote signal strength - uint8_t txbuf; ///< how full the tx buffer is as a percentage - uint8_t noise; ///< background noise level - uint8_t remnoise; ///< remote background noise level -} mavlink_radio_t; - -#define MAVLINK_MSG_ID_RADIO_LEN 9 -#define MAVLINK_MSG_ID_166_LEN 9 - -#define MAVLINK_MSG_ID_RADIO_CRC 21 -#define MAVLINK_MSG_ID_166_CRC 21 - - - -#define MAVLINK_MESSAGE_INFO_RADIO { \ - "RADIO", \ - 7, \ - { { "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_radio_t, rxerrors) }, \ - { "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_radio_t, fixed) }, \ - { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_radio_t, rssi) }, \ - { "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_radio_t, remrssi) }, \ - { "txbuf", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_radio_t, txbuf) }, \ - { "noise", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_radio_t, noise) }, \ - { "remnoise", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_radio_t, remnoise) }, \ - } \ -} - - -/** - * @brief Pack a radio message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param rssi local signal strength - * @param remrssi remote signal strength - * @param txbuf how full the tx buffer is as a percentage - * @param noise background noise level - * @param remnoise remote background noise level - * @param rxerrors receive errors - * @param fixed count of error corrected packets - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RADIO_LEN]; - _mav_put_uint16_t(buf, 0, rxerrors); - _mav_put_uint16_t(buf, 2, fixed); - _mav_put_uint8_t(buf, 4, rssi); - _mav_put_uint8_t(buf, 5, remrssi); - _mav_put_uint8_t(buf, 6, txbuf); - _mav_put_uint8_t(buf, 7, noise); - _mav_put_uint8_t(buf, 8, remnoise); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_LEN); -#else - mavlink_radio_t packet; - packet.rxerrors = rxerrors; - packet.fixed = fixed; - packet.rssi = rssi; - packet.remrssi = remrssi; - packet.txbuf = txbuf; - packet.noise = noise; - packet.remnoise = remnoise; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RADIO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_LEN); -#endif -} - -/** - * @brief Pack a radio message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rssi local signal strength - * @param remrssi remote signal strength - * @param txbuf how full the tx buffer is as a percentage - * @param noise background noise level - * @param remnoise remote background noise level - * @param rxerrors receive errors - * @param fixed count of error corrected packets - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t rssi,uint8_t remrssi,uint8_t txbuf,uint8_t noise,uint8_t remnoise,uint16_t rxerrors,uint16_t fixed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RADIO_LEN]; - _mav_put_uint16_t(buf, 0, rxerrors); - _mav_put_uint16_t(buf, 2, fixed); - _mav_put_uint8_t(buf, 4, rssi); - _mav_put_uint8_t(buf, 5, remrssi); - _mav_put_uint8_t(buf, 6, txbuf); - _mav_put_uint8_t(buf, 7, noise); - _mav_put_uint8_t(buf, 8, remnoise); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_LEN); -#else - mavlink_radio_t packet; - packet.rxerrors = rxerrors; - packet.fixed = fixed; - packet.rssi = rssi; - packet.remrssi = remrssi; - packet.txbuf = txbuf; - packet.noise = noise; - packet.remnoise = remnoise; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RADIO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_LEN); -#endif -} - -/** - * @brief Encode a radio struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param radio C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_t* radio) -{ - return mavlink_msg_radio_pack(system_id, component_id, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed); -} - -/** - * @brief Encode a radio struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param radio C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_radio_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_t* radio) -{ - return mavlink_msg_radio_pack_chan(system_id, component_id, chan, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed); -} - -/** - * @brief Send a radio message - * @param chan MAVLink channel to send the message - * - * @param rssi local signal strength - * @param remrssi remote signal strength - * @param txbuf how full the tx buffer is as a percentage - * @param noise background noise level - * @param remnoise remote background noise level - * @param rxerrors receive errors - * @param fixed count of error corrected packets - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RADIO_LEN]; - _mav_put_uint16_t(buf, 0, rxerrors); - _mav_put_uint16_t(buf, 2, fixed); - _mav_put_uint8_t(buf, 4, rssi); - _mav_put_uint8_t(buf, 5, remrssi); - _mav_put_uint8_t(buf, 6, txbuf); - _mav_put_uint8_t(buf, 7, noise); - _mav_put_uint8_t(buf, 8, remnoise); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN); -#endif -#else - mavlink_radio_t packet; - packet.rxerrors = rxerrors; - packet.fixed = fixed; - packet.rssi = rssi; - packet.remrssi = remrssi; - packet.txbuf = txbuf; - packet.noise = noise; - packet.remnoise = remnoise; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, MAVLINK_MSG_ID_RADIO_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_RADIO_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_radio_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, rxerrors); - _mav_put_uint16_t(buf, 2, fixed); - _mav_put_uint8_t(buf, 4, rssi); - _mav_put_uint8_t(buf, 5, remrssi); - _mav_put_uint8_t(buf, 6, txbuf); - _mav_put_uint8_t(buf, 7, noise); - _mav_put_uint8_t(buf, 8, remnoise); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN); -#endif -#else - mavlink_radio_t *packet = (mavlink_radio_t *)msgbuf; - packet->rxerrors = rxerrors; - packet->fixed = fixed; - packet->rssi = rssi; - packet->remrssi = remrssi; - packet->txbuf = txbuf; - packet->noise = noise; - packet->remnoise = remnoise; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)packet, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)packet, MAVLINK_MSG_ID_RADIO_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE RADIO UNPACKING - - -/** - * @brief Get field rssi from radio message - * - * @return local signal strength - */ -static inline uint8_t mavlink_msg_radio_get_rssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field remrssi from radio message - * - * @return remote signal strength - */ -static inline uint8_t mavlink_msg_radio_get_remrssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field txbuf from radio message - * - * @return how full the tx buffer is as a percentage - */ -static inline uint8_t mavlink_msg_radio_get_txbuf(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field noise from radio message - * - * @return background noise level - */ -static inline uint8_t mavlink_msg_radio_get_noise(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field remnoise from radio message - * - * @return remote background noise level - */ -static inline uint8_t mavlink_msg_radio_get_remnoise(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field rxerrors from radio message - * - * @return receive errors - */ -static inline uint16_t mavlink_msg_radio_get_rxerrors(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field fixed from radio message - * - * @return count of error corrected packets - */ -static inline uint16_t mavlink_msg_radio_get_fixed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Decode a radio message into a struct - * - * @param msg The message to decode - * @param radio C-struct to decode the message contents into - */ -static inline void mavlink_msg_radio_decode(const mavlink_message_t* msg, mavlink_radio_t* radio) -{ -#if MAVLINK_NEED_BYTE_SWAP - radio->rxerrors = mavlink_msg_radio_get_rxerrors(msg); - radio->fixed = mavlink_msg_radio_get_fixed(msg); - radio->rssi = mavlink_msg_radio_get_rssi(msg); - radio->remrssi = mavlink_msg_radio_get_remrssi(msg); - radio->txbuf = mavlink_msg_radio_get_txbuf(msg); - radio->noise = mavlink_msg_radio_get_noise(msg); - radio->remnoise = mavlink_msg_radio_get_remnoise(msg); -#else - memcpy(radio, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RADIO_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h deleted file mode 100644 index 4598251fe..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE RALLY_FETCH_POINT PACKING - -#define MAVLINK_MSG_ID_RALLY_FETCH_POINT 176 - -typedef struct __mavlink_rally_fetch_point_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t idx; ///< point index (first point is 0) -} mavlink_rally_fetch_point_t; - -#define MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN 3 -#define MAVLINK_MSG_ID_176_LEN 3 - -#define MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC 234 -#define MAVLINK_MSG_ID_176_CRC 234 - - - -#define MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT { \ - "RALLY_FETCH_POINT", \ - 3, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rally_fetch_point_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_rally_fetch_point_t, target_component) }, \ - { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_rally_fetch_point_t, idx) }, \ - } \ -} - - -/** - * @brief Pack a rally_fetch_point message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 0) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rally_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#else - mavlink_rally_fetch_point_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RALLY_FETCH_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif -} - -/** - * @brief Pack a rally_fetch_point message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 0) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rally_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#else - mavlink_rally_fetch_point_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RALLY_FETCH_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif -} - -/** - * @brief Encode a rally_fetch_point struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param rally_fetch_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rally_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rally_fetch_point_t* rally_fetch_point) -{ - return mavlink_msg_rally_fetch_point_pack(system_id, component_id, msg, rally_fetch_point->target_system, rally_fetch_point->target_component, rally_fetch_point->idx); -} - -/** - * @brief Encode a rally_fetch_point struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rally_fetch_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rally_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rally_fetch_point_t* rally_fetch_point) -{ - return mavlink_msg_rally_fetch_point_pack_chan(system_id, component_id, chan, msg, rally_fetch_point->target_system, rally_fetch_point->target_component, rally_fetch_point->idx); -} - -/** - * @brief Send a rally_fetch_point message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 0) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_rally_fetch_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif -#else - mavlink_rally_fetch_point_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_rally_fetch_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, idx); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif -#else - mavlink_rally_fetch_point_t *packet = (mavlink_rally_fetch_point_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - packet->idx = idx; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE RALLY_FETCH_POINT UNPACKING - - -/** - * @brief Get field target_system from rally_fetch_point message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_rally_fetch_point_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from rally_fetch_point message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_rally_fetch_point_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field idx from rally_fetch_point message - * - * @return point index (first point is 0) - */ -static inline uint8_t mavlink_msg_rally_fetch_point_get_idx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a rally_fetch_point message into a struct - * - * @param msg The message to decode - * @param rally_fetch_point C-struct to decode the message contents into - */ -static inline void mavlink_msg_rally_fetch_point_decode(const mavlink_message_t* msg, mavlink_rally_fetch_point_t* rally_fetch_point) -{ -#if MAVLINK_NEED_BYTE_SWAP - rally_fetch_point->target_system = mavlink_msg_rally_fetch_point_get_target_system(msg); - rally_fetch_point->target_component = mavlink_msg_rally_fetch_point_get_target_component(msg); - rally_fetch_point->idx = mavlink_msg_rally_fetch_point_get_idx(msg); -#else - memcpy(rally_fetch_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h deleted file mode 100644 index b44e764a9..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h +++ /dev/null @@ -1,425 +0,0 @@ -// MESSAGE RALLY_POINT PACKING - -#define MAVLINK_MSG_ID_RALLY_POINT 175 - -typedef struct __mavlink_rally_point_t -{ - int32_t lat; ///< Latitude of point in degrees * 1E7 - int32_t lng; ///< Longitude of point in degrees * 1E7 - int16_t alt; ///< Transit / loiter altitude in meters relative to home - int16_t break_alt; ///< Break altitude in meters relative to home - uint16_t land_dir; ///< Heading to aim for when landing. In centi-degrees. - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t idx; ///< point index (first point is 0) - uint8_t count; ///< total number of points (for sanity checking) - uint8_t flags; ///< See RALLY_FLAGS enum for definition of the bitmask. -} mavlink_rally_point_t; - -#define MAVLINK_MSG_ID_RALLY_POINT_LEN 19 -#define MAVLINK_MSG_ID_175_LEN 19 - -#define MAVLINK_MSG_ID_RALLY_POINT_CRC 138 -#define MAVLINK_MSG_ID_175_CRC 138 - - - -#define MAVLINK_MESSAGE_INFO_RALLY_POINT { \ - "RALLY_POINT", \ - 10, \ - { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_rally_point_t, lat) }, \ - { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_rally_point_t, lng) }, \ - { "alt", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_rally_point_t, alt) }, \ - { "break_alt", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_rally_point_t, break_alt) }, \ - { "land_dir", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rally_point_t, land_dir) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_rally_point_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_rally_point_t, target_component) }, \ - { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rally_point_t, idx) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rally_point_t, count) }, \ - { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_rally_point_t, flags) }, \ - } \ -} - - -/** - * @brief Pack a rally_point message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 0) - * @param count total number of points (for sanity checking) - * @param lat Latitude of point in degrees * 1E7 - * @param lng Longitude of point in degrees * 1E7 - * @param alt Transit / loiter altitude in meters relative to home - * @param break_alt Break altitude in meters relative to home - * @param land_dir Heading to aim for when landing. In centi-degrees. - * @param flags See RALLY_FLAGS enum for definition of the bitmask. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rally_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN]; - _mav_put_int32_t(buf, 0, lat); - _mav_put_int32_t(buf, 4, lng); - _mav_put_int16_t(buf, 8, alt); - _mav_put_int16_t(buf, 10, break_alt); - _mav_put_uint16_t(buf, 12, land_dir); - _mav_put_uint8_t(buf, 14, target_system); - _mav_put_uint8_t(buf, 15, target_component); - _mav_put_uint8_t(buf, 16, idx); - _mav_put_uint8_t(buf, 17, count); - _mav_put_uint8_t(buf, 18, flags); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#else - mavlink_rally_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.alt = alt; - packet.break_alt = break_alt; - packet.land_dir = land_dir; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - packet.count = count; - packet.flags = flags; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RALLY_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif -} - -/** - * @brief Pack a rally_point message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 0) - * @param count total number of points (for sanity checking) - * @param lat Latitude of point in degrees * 1E7 - * @param lng Longitude of point in degrees * 1E7 - * @param alt Transit / loiter altitude in meters relative to home - * @param break_alt Break altitude in meters relative to home - * @param land_dir Heading to aim for when landing. In centi-degrees. - * @param flags See RALLY_FLAGS enum for definition of the bitmask. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rally_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t idx,uint8_t count,int32_t lat,int32_t lng,int16_t alt,int16_t break_alt,uint16_t land_dir,uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN]; - _mav_put_int32_t(buf, 0, lat); - _mav_put_int32_t(buf, 4, lng); - _mav_put_int16_t(buf, 8, alt); - _mav_put_int16_t(buf, 10, break_alt); - _mav_put_uint16_t(buf, 12, land_dir); - _mav_put_uint8_t(buf, 14, target_system); - _mav_put_uint8_t(buf, 15, target_component); - _mav_put_uint8_t(buf, 16, idx); - _mav_put_uint8_t(buf, 17, count); - _mav_put_uint8_t(buf, 18, flags); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#else - mavlink_rally_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.alt = alt; - packet.break_alt = break_alt; - packet.land_dir = land_dir; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - packet.count = count; - packet.flags = flags; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RALLY_POINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif -} - -/** - * @brief Encode a rally_point struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param rally_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rally_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rally_point_t* rally_point) -{ - return mavlink_msg_rally_point_pack(system_id, component_id, msg, rally_point->target_system, rally_point->target_component, rally_point->idx, rally_point->count, rally_point->lat, rally_point->lng, rally_point->alt, rally_point->break_alt, rally_point->land_dir, rally_point->flags); -} - -/** - * @brief Encode a rally_point struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rally_point C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rally_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rally_point_t* rally_point) -{ - return mavlink_msg_rally_point_pack_chan(system_id, component_id, chan, msg, rally_point->target_system, rally_point->target_component, rally_point->idx, rally_point->count, rally_point->lat, rally_point->lng, rally_point->alt, rally_point->break_alt, rally_point->land_dir, rally_point->flags); -} - -/** - * @brief Send a rally_point message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param idx point index (first point is 0) - * @param count total number of points (for sanity checking) - * @param lat Latitude of point in degrees * 1E7 - * @param lng Longitude of point in degrees * 1E7 - * @param alt Transit / loiter altitude in meters relative to home - * @param break_alt Break altitude in meters relative to home - * @param land_dir Heading to aim for when landing. In centi-degrees. - * @param flags See RALLY_FLAGS enum for definition of the bitmask. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_rally_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN]; - _mav_put_int32_t(buf, 0, lat); - _mav_put_int32_t(buf, 4, lng); - _mav_put_int16_t(buf, 8, alt); - _mav_put_int16_t(buf, 10, break_alt); - _mav_put_uint16_t(buf, 12, land_dir); - _mav_put_uint8_t(buf, 14, target_system); - _mav_put_uint8_t(buf, 15, target_component); - _mav_put_uint8_t(buf, 16, idx); - _mav_put_uint8_t(buf, 17, count); - _mav_put_uint8_t(buf, 18, flags); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif -#else - mavlink_rally_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.alt = alt; - packet.break_alt = break_alt; - packet.land_dir = land_dir; - packet.target_system = target_system; - packet.target_component = target_component; - packet.idx = idx; - packet.count = count; - packet.flags = flags; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_RALLY_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_rally_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, lat); - _mav_put_int32_t(buf, 4, lng); - _mav_put_int16_t(buf, 8, alt); - _mav_put_int16_t(buf, 10, break_alt); - _mav_put_uint16_t(buf, 12, land_dir); - _mav_put_uint8_t(buf, 14, target_system); - _mav_put_uint8_t(buf, 15, target_component); - _mav_put_uint8_t(buf, 16, idx); - _mav_put_uint8_t(buf, 17, count); - _mav_put_uint8_t(buf, 18, flags); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif -#else - mavlink_rally_point_t *packet = (mavlink_rally_point_t *)msgbuf; - packet->lat = lat; - packet->lng = lng; - packet->alt = alt; - packet->break_alt = break_alt; - packet->land_dir = land_dir; - packet->target_system = target_system; - packet->target_component = target_component; - packet->idx = idx; - packet->count = count; - packet->flags = flags; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE RALLY_POINT UNPACKING - - -/** - * @brief Get field target_system from rally_point message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_rally_point_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field target_component from rally_point message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_rally_point_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 15); -} - -/** - * @brief Get field idx from rally_point message - * - * @return point index (first point is 0) - */ -static inline uint8_t mavlink_msg_rally_point_get_idx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field count from rally_point message - * - * @return total number of points (for sanity checking) - */ -static inline uint8_t mavlink_msg_rally_point_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field lat from rally_point message - * - * @return Latitude of point in degrees * 1E7 - */ -static inline int32_t mavlink_msg_rally_point_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field lng from rally_point message - * - * @return Longitude of point in degrees * 1E7 - */ -static inline int32_t mavlink_msg_rally_point_get_lng(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field alt from rally_point message - * - * @return Transit / loiter altitude in meters relative to home - */ -static inline int16_t mavlink_msg_rally_point_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field break_alt from rally_point message - * - * @return Break altitude in meters relative to home - */ -static inline int16_t mavlink_msg_rally_point_get_break_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 10); -} - -/** - * @brief Get field land_dir from rally_point message - * - * @return Heading to aim for when landing. In centi-degrees. - */ -static inline uint16_t mavlink_msg_rally_point_get_land_dir(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field flags from rally_point message - * - * @return See RALLY_FLAGS enum for definition of the bitmask. - */ -static inline uint8_t mavlink_msg_rally_point_get_flags(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 18); -} - -/** - * @brief Decode a rally_point message into a struct - * - * @param msg The message to decode - * @param rally_point C-struct to decode the message contents into - */ -static inline void mavlink_msg_rally_point_decode(const mavlink_message_t* msg, mavlink_rally_point_t* rally_point) -{ -#if MAVLINK_NEED_BYTE_SWAP - rally_point->lat = mavlink_msg_rally_point_get_lat(msg); - rally_point->lng = mavlink_msg_rally_point_get_lng(msg); - rally_point->alt = mavlink_msg_rally_point_get_alt(msg); - rally_point->break_alt = mavlink_msg_rally_point_get_break_alt(msg); - rally_point->land_dir = mavlink_msg_rally_point_get_land_dir(msg); - rally_point->target_system = mavlink_msg_rally_point_get_target_system(msg); - rally_point->target_component = mavlink_msg_rally_point_get_target_component(msg); - rally_point->idx = mavlink_msg_rally_point_get_idx(msg); - rally_point->count = mavlink_msg_rally_point_get_count(msg); - rally_point->flags = mavlink_msg_rally_point_get_flags(msg); -#else - memcpy(rally_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RALLY_POINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h deleted file mode 100644 index 464ce8a47..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE RANGEFINDER PACKING - -#define MAVLINK_MSG_ID_RANGEFINDER 173 - -typedef struct __mavlink_rangefinder_t -{ - float distance; ///< distance in meters - float voltage; ///< raw voltage if available, zero otherwise -} mavlink_rangefinder_t; - -#define MAVLINK_MSG_ID_RANGEFINDER_LEN 8 -#define MAVLINK_MSG_ID_173_LEN 8 - -#define MAVLINK_MSG_ID_RANGEFINDER_CRC 83 -#define MAVLINK_MSG_ID_173_CRC 83 - - - -#define MAVLINK_MESSAGE_INFO_RANGEFINDER { \ - "RANGEFINDER", \ - 2, \ - { { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_rangefinder_t, distance) }, \ - { "voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_rangefinder_t, voltage) }, \ - } \ -} - - -/** - * @brief Pack a rangefinder message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param distance distance in meters - * @param voltage raw voltage if available, zero otherwise - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rangefinder_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float distance, float voltage) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN]; - _mav_put_float(buf, 0, distance); - _mav_put_float(buf, 4, voltage); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#else - mavlink_rangefinder_t packet; - packet.distance = distance; - packet.voltage = voltage; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RANGEFINDER; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif -} - -/** - * @brief Pack a rangefinder message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param distance distance in meters - * @param voltage raw voltage if available, zero otherwise - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rangefinder_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float distance,float voltage) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN]; - _mav_put_float(buf, 0, distance); - _mav_put_float(buf, 4, voltage); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#else - mavlink_rangefinder_t packet; - packet.distance = distance; - packet.voltage = voltage; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RANGEFINDER; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif -} - -/** - * @brief Encode a rangefinder struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param rangefinder C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rangefinder_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder) -{ - return mavlink_msg_rangefinder_pack(system_id, component_id, msg, rangefinder->distance, rangefinder->voltage); -} - -/** - * @brief Encode a rangefinder struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rangefinder C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rangefinder_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder) -{ - return mavlink_msg_rangefinder_pack_chan(system_id, component_id, chan, msg, rangefinder->distance, rangefinder->voltage); -} - -/** - * @brief Send a rangefinder message - * @param chan MAVLink channel to send the message - * - * @param distance distance in meters - * @param voltage raw voltage if available, zero otherwise - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_rangefinder_send(mavlink_channel_t chan, float distance, float voltage) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN]; - _mav_put_float(buf, 0, distance); - _mav_put_float(buf, 4, voltage); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif -#else - mavlink_rangefinder_t packet; - packet.distance = distance; - packet.voltage = voltage; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)&packet, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)&packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_RANGEFINDER_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_rangefinder_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float distance, float voltage) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, distance); - _mav_put_float(buf, 4, voltage); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif -#else - mavlink_rangefinder_t *packet = (mavlink_rangefinder_t *)msgbuf; - packet->distance = distance; - packet->voltage = voltage; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)packet, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE RANGEFINDER UNPACKING - - -/** - * @brief Get field distance from rangefinder message - * - * @return distance in meters - */ -static inline float mavlink_msg_rangefinder_get_distance(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field voltage from rangefinder message - * - * @return raw voltage if available, zero otherwise - */ -static inline float mavlink_msg_rangefinder_get_voltage(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Decode a rangefinder message into a struct - * - * @param msg The message to decode - * @param rangefinder C-struct to decode the message contents into - */ -static inline void mavlink_msg_rangefinder_decode(const mavlink_message_t* msg, mavlink_rangefinder_t* rangefinder) -{ -#if MAVLINK_NEED_BYTE_SWAP - rangefinder->distance = mavlink_msg_rangefinder_get_distance(msg); - rangefinder->voltage = mavlink_msg_rangefinder_get_voltage(msg); -#else - memcpy(rangefinder, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RANGEFINDER_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h deleted file mode 100644 index d18f31c95..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h +++ /dev/null @@ -1,473 +0,0 @@ -// MESSAGE SENSOR_OFFSETS PACKING - -#define MAVLINK_MSG_ID_SENSOR_OFFSETS 150 - -typedef struct __mavlink_sensor_offsets_t -{ - float mag_declination; ///< magnetic declination (radians) - int32_t raw_press; ///< raw pressure from barometer - int32_t raw_temp; ///< raw temperature from barometer - float gyro_cal_x; ///< gyro X calibration - float gyro_cal_y; ///< gyro Y calibration - float gyro_cal_z; ///< gyro Z calibration - float accel_cal_x; ///< accel X calibration - float accel_cal_y; ///< accel Y calibration - float accel_cal_z; ///< accel Z calibration - int16_t mag_ofs_x; ///< magnetometer X offset - int16_t mag_ofs_y; ///< magnetometer Y offset - int16_t mag_ofs_z; ///< magnetometer Z offset -} mavlink_sensor_offsets_t; - -#define MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN 42 -#define MAVLINK_MSG_ID_150_LEN 42 - -#define MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC 134 -#define MAVLINK_MSG_ID_150_CRC 134 - - - -#define MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS { \ - "SENSOR_OFFSETS", \ - 12, \ - { { "mag_declination", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sensor_offsets_t, mag_declination) }, \ - { "raw_press", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_sensor_offsets_t, raw_press) }, \ - { "raw_temp", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_sensor_offsets_t, raw_temp) }, \ - { "gyro_cal_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sensor_offsets_t, gyro_cal_x) }, \ - { "gyro_cal_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sensor_offsets_t, gyro_cal_y) }, \ - { "gyro_cal_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sensor_offsets_t, gyro_cal_z) }, \ - { "accel_cal_x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_sensor_offsets_t, accel_cal_x) }, \ - { "accel_cal_y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_sensor_offsets_t, accel_cal_y) }, \ - { "accel_cal_z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_sensor_offsets_t, accel_cal_z) }, \ - { "mag_ofs_x", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_sensor_offsets_t, mag_ofs_x) }, \ - { "mag_ofs_y", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_sensor_offsets_t, mag_ofs_y) }, \ - { "mag_ofs_z", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_sensor_offsets_t, mag_ofs_z) }, \ - } \ -} - - -/** - * @brief Pack a sensor_offsets message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - * @param mag_declination magnetic declination (radians) - * @param raw_press raw pressure from barometer - * @param raw_temp raw temperature from barometer - * @param gyro_cal_x gyro X calibration - * @param gyro_cal_y gyro Y calibration - * @param gyro_cal_z gyro Z calibration - * @param accel_cal_x accel X calibration - * @param accel_cal_y accel Y calibration - * @param accel_cal_z accel Z calibration - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN]; - _mav_put_float(buf, 0, mag_declination); - _mav_put_int32_t(buf, 4, raw_press); - _mav_put_int32_t(buf, 8, raw_temp); - _mav_put_float(buf, 12, gyro_cal_x); - _mav_put_float(buf, 16, gyro_cal_y); - _mav_put_float(buf, 20, gyro_cal_z); - _mav_put_float(buf, 24, accel_cal_x); - _mav_put_float(buf, 28, accel_cal_y); - _mav_put_float(buf, 32, accel_cal_z); - _mav_put_int16_t(buf, 36, mag_ofs_x); - _mav_put_int16_t(buf, 38, mag_ofs_y); - _mav_put_int16_t(buf, 40, mag_ofs_z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#else - mavlink_sensor_offsets_t packet; - packet.mag_declination = mag_declination; - packet.raw_press = raw_press; - packet.raw_temp = raw_temp; - packet.gyro_cal_x = gyro_cal_x; - packet.gyro_cal_y = gyro_cal_y; - packet.gyro_cal_z = gyro_cal_z; - packet.accel_cal_x = accel_cal_x; - packet.accel_cal_y = accel_cal_y; - packet.accel_cal_z = accel_cal_z; - packet.mag_ofs_x = mag_ofs_x; - packet.mag_ofs_y = mag_ofs_y; - packet.mag_ofs_z = mag_ofs_z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif -} - -/** - * @brief Pack a sensor_offsets message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - * @param mag_declination magnetic declination (radians) - * @param raw_press raw pressure from barometer - * @param raw_temp raw temperature from barometer - * @param gyro_cal_x gyro X calibration - * @param gyro_cal_y gyro Y calibration - * @param gyro_cal_z gyro Z calibration - * @param accel_cal_x accel X calibration - * @param accel_cal_y accel Y calibration - * @param accel_cal_z accel Z calibration - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z,float mag_declination,int32_t raw_press,int32_t raw_temp,float gyro_cal_x,float gyro_cal_y,float gyro_cal_z,float accel_cal_x,float accel_cal_y,float accel_cal_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN]; - _mav_put_float(buf, 0, mag_declination); - _mav_put_int32_t(buf, 4, raw_press); - _mav_put_int32_t(buf, 8, raw_temp); - _mav_put_float(buf, 12, gyro_cal_x); - _mav_put_float(buf, 16, gyro_cal_y); - _mav_put_float(buf, 20, gyro_cal_z); - _mav_put_float(buf, 24, accel_cal_x); - _mav_put_float(buf, 28, accel_cal_y); - _mav_put_float(buf, 32, accel_cal_z); - _mav_put_int16_t(buf, 36, mag_ofs_x); - _mav_put_int16_t(buf, 38, mag_ofs_y); - _mav_put_int16_t(buf, 40, mag_ofs_z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#else - mavlink_sensor_offsets_t packet; - packet.mag_declination = mag_declination; - packet.raw_press = raw_press; - packet.raw_temp = raw_temp; - packet.gyro_cal_x = gyro_cal_x; - packet.gyro_cal_y = gyro_cal_y; - packet.gyro_cal_z = gyro_cal_z; - packet.accel_cal_x = accel_cal_x; - packet.accel_cal_y = accel_cal_y; - packet.accel_cal_z = accel_cal_z; - packet.mag_ofs_x = mag_ofs_x; - packet.mag_ofs_y = mag_ofs_y; - packet.mag_ofs_z = mag_ofs_z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif -} - -/** - * @brief Encode a sensor_offsets struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param sensor_offsets C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets) -{ - return mavlink_msg_sensor_offsets_pack(system_id, component_id, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z); -} - -/** - * @brief Encode a sensor_offsets struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sensor_offsets C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sensor_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets) -{ - return mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, chan, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z); -} - -/** - * @brief Send a sensor_offsets message - * @param chan MAVLink channel to send the message - * - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - * @param mag_declination magnetic declination (radians) - * @param raw_press raw pressure from barometer - * @param raw_temp raw temperature from barometer - * @param gyro_cal_x gyro X calibration - * @param gyro_cal_y gyro Y calibration - * @param gyro_cal_z gyro Z calibration - * @param accel_cal_x accel X calibration - * @param accel_cal_y accel Y calibration - * @param accel_cal_z accel Z calibration - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN]; - _mav_put_float(buf, 0, mag_declination); - _mav_put_int32_t(buf, 4, raw_press); - _mav_put_int32_t(buf, 8, raw_temp); - _mav_put_float(buf, 12, gyro_cal_x); - _mav_put_float(buf, 16, gyro_cal_y); - _mav_put_float(buf, 20, gyro_cal_z); - _mav_put_float(buf, 24, accel_cal_x); - _mav_put_float(buf, 28, accel_cal_y); - _mav_put_float(buf, 32, accel_cal_z); - _mav_put_int16_t(buf, 36, mag_ofs_x); - _mav_put_int16_t(buf, 38, mag_ofs_y); - _mav_put_int16_t(buf, 40, mag_ofs_z); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif -#else - mavlink_sensor_offsets_t packet; - packet.mag_declination = mag_declination; - packet.raw_press = raw_press; - packet.raw_temp = raw_temp; - packet.gyro_cal_x = gyro_cal_x; - packet.gyro_cal_y = gyro_cal_y; - packet.gyro_cal_z = gyro_cal_z; - packet.accel_cal_x = accel_cal_x; - packet.accel_cal_y = accel_cal_y; - packet.accel_cal_z = accel_cal_z; - packet.mag_ofs_x = mag_ofs_x; - packet.mag_ofs_y = mag_ofs_y; - packet.mag_ofs_z = mag_ofs_z; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_sensor_offsets_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, mag_declination); - _mav_put_int32_t(buf, 4, raw_press); - _mav_put_int32_t(buf, 8, raw_temp); - _mav_put_float(buf, 12, gyro_cal_x); - _mav_put_float(buf, 16, gyro_cal_y); - _mav_put_float(buf, 20, gyro_cal_z); - _mav_put_float(buf, 24, accel_cal_x); - _mav_put_float(buf, 28, accel_cal_y); - _mav_put_float(buf, 32, accel_cal_z); - _mav_put_int16_t(buf, 36, mag_ofs_x); - _mav_put_int16_t(buf, 38, mag_ofs_y); - _mav_put_int16_t(buf, 40, mag_ofs_z); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif -#else - mavlink_sensor_offsets_t *packet = (mavlink_sensor_offsets_t *)msgbuf; - packet->mag_declination = mag_declination; - packet->raw_press = raw_press; - packet->raw_temp = raw_temp; - packet->gyro_cal_x = gyro_cal_x; - packet->gyro_cal_y = gyro_cal_y; - packet->gyro_cal_z = gyro_cal_z; - packet->accel_cal_x = accel_cal_x; - packet->accel_cal_y = accel_cal_y; - packet->accel_cal_z = accel_cal_z; - packet->mag_ofs_x = mag_ofs_x; - packet->mag_ofs_y = mag_ofs_y; - packet->mag_ofs_z = mag_ofs_z; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SENSOR_OFFSETS UNPACKING - - -/** - * @brief Get field mag_ofs_x from sensor_offsets message - * - * @return magnetometer X offset - */ -static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 36); -} - -/** - * @brief Get field mag_ofs_y from sensor_offsets message - * - * @return magnetometer Y offset - */ -static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 38); -} - -/** - * @brief Get field mag_ofs_z from sensor_offsets message - * - * @return magnetometer Z offset - */ -static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 40); -} - -/** - * @brief Get field mag_declination from sensor_offsets message - * - * @return magnetic declination (radians) - */ -static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field raw_press from sensor_offsets message - * - * @return raw pressure from barometer - */ -static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field raw_temp from sensor_offsets message - * - * @return raw temperature from barometer - */ -static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field gyro_cal_x from sensor_offsets message - * - * @return gyro X calibration - */ -static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field gyro_cal_y from sensor_offsets message - * - * @return gyro Y calibration - */ -static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field gyro_cal_z from sensor_offsets message - * - * @return gyro Z calibration - */ -static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field accel_cal_x from sensor_offsets message - * - * @return accel X calibration - */ -static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field accel_cal_y from sensor_offsets message - * - * @return accel Y calibration - */ -static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field accel_cal_z from sensor_offsets message - * - * @return accel Z calibration - */ -static inline float mavlink_msg_sensor_offsets_get_accel_cal_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Decode a sensor_offsets message into a struct - * - * @param msg The message to decode - * @param sensor_offsets C-struct to decode the message contents into - */ -static inline void mavlink_msg_sensor_offsets_decode(const mavlink_message_t* msg, mavlink_sensor_offsets_t* sensor_offsets) -{ -#if MAVLINK_NEED_BYTE_SWAP - sensor_offsets->mag_declination = mavlink_msg_sensor_offsets_get_mag_declination(msg); - sensor_offsets->raw_press = mavlink_msg_sensor_offsets_get_raw_press(msg); - sensor_offsets->raw_temp = mavlink_msg_sensor_offsets_get_raw_temp(msg); - sensor_offsets->gyro_cal_x = mavlink_msg_sensor_offsets_get_gyro_cal_x(msg); - sensor_offsets->gyro_cal_y = mavlink_msg_sensor_offsets_get_gyro_cal_y(msg); - sensor_offsets->gyro_cal_z = mavlink_msg_sensor_offsets_get_gyro_cal_z(msg); - sensor_offsets->accel_cal_x = mavlink_msg_sensor_offsets_get_accel_cal_x(msg); - sensor_offsets->accel_cal_y = mavlink_msg_sensor_offsets_get_accel_cal_y(msg); - sensor_offsets->accel_cal_z = mavlink_msg_sensor_offsets_get_accel_cal_z(msg); - sensor_offsets->mag_ofs_x = mavlink_msg_sensor_offsets_get_mag_ofs_x(msg); - sensor_offsets->mag_ofs_y = mavlink_msg_sensor_offsets_get_mag_ofs_y(msg); - sensor_offsets->mag_ofs_z = mavlink_msg_sensor_offsets_get_mag_ofs_z(msg); -#else - memcpy(sensor_offsets, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h deleted file mode 100644 index fc6aa71e0..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE SET_MAG_OFFSETS PACKING - -#define MAVLINK_MSG_ID_SET_MAG_OFFSETS 151 - -typedef struct __mavlink_set_mag_offsets_t -{ - int16_t mag_ofs_x; ///< magnetometer X offset - int16_t mag_ofs_y; ///< magnetometer Y offset - int16_t mag_ofs_z; ///< magnetometer Z offset - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_set_mag_offsets_t; - -#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN 8 -#define MAVLINK_MSG_ID_151_LEN 8 - -#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC 219 -#define MAVLINK_MSG_ID_151_CRC 219 - - - -#define MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS { \ - "SET_MAG_OFFSETS", \ - 5, \ - { { "mag_ofs_x", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_set_mag_offsets_t, mag_ofs_x) }, \ - { "mag_ofs_y", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_set_mag_offsets_t, mag_ofs_y) }, \ - { "mag_ofs_z", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_set_mag_offsets_t, mag_ofs_z) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_set_mag_offsets_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_set_mag_offsets_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a set_mag_offsets message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN]; - _mav_put_int16_t(buf, 0, mag_ofs_x); - _mav_put_int16_t(buf, 2, mag_ofs_y); - _mav_put_int16_t(buf, 4, mag_ofs_z); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#else - mavlink_set_mag_offsets_t packet; - packet.mag_ofs_x = mag_ofs_x; - packet.mag_ofs_y = mag_ofs_y; - packet.mag_ofs_z = mag_ofs_z; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif -} - -/** - * @brief Pack a set_mag_offsets message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN]; - _mav_put_int16_t(buf, 0, mag_ofs_x); - _mav_put_int16_t(buf, 2, mag_ofs_y); - _mav_put_int16_t(buf, 4, mag_ofs_z); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#else - mavlink_set_mag_offsets_t packet; - packet.mag_ofs_x = mag_ofs_x; - packet.mag_ofs_y = mag_ofs_y; - packet.mag_ofs_z = mag_ofs_z; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif -} - -/** - * @brief Encode a set_mag_offsets struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_mag_offsets C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets) -{ - return mavlink_msg_set_mag_offsets_pack(system_id, component_id, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z); -} - -/** - * @brief Encode a set_mag_offsets struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_mag_offsets C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_mag_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets) -{ - return mavlink_msg_set_mag_offsets_pack_chan(system_id, component_id, chan, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z); -} - -/** - * @brief Send a set_mag_offsets message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN]; - _mav_put_int16_t(buf, 0, mag_ofs_x); - _mav_put_int16_t(buf, 2, mag_ofs_y); - _mav_put_int16_t(buf, 4, mag_ofs_z); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif -#else - mavlink_set_mag_offsets_t packet; - packet.mag_ofs_x = mag_ofs_x; - packet.mag_ofs_y = mag_ofs_y; - packet.mag_ofs_z = mag_ofs_z; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_set_mag_offsets_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int16_t(buf, 0, mag_ofs_x); - _mav_put_int16_t(buf, 2, mag_ofs_y); - _mav_put_int16_t(buf, 4, mag_ofs_z); - _mav_put_uint8_t(buf, 6, target_system); - _mav_put_uint8_t(buf, 7, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif -#else - mavlink_set_mag_offsets_t *packet = (mavlink_set_mag_offsets_t *)msgbuf; - packet->mag_ofs_x = mag_ofs_x; - packet->mag_ofs_y = mag_ofs_y; - packet->mag_ofs_z = mag_ofs_z; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SET_MAG_OFFSETS UNPACKING - - -/** - * @brief Get field target_system from set_mag_offsets message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field target_component from set_mag_offsets message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field mag_ofs_x from set_mag_offsets message - * - * @return magnetometer X offset - */ -static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 0); -} - -/** - * @brief Get field mag_ofs_y from set_mag_offsets message - * - * @return magnetometer Y offset - */ -static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 2); -} - -/** - * @brief Get field mag_ofs_z from set_mag_offsets message - * - * @return magnetometer Z offset - */ -static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 4); -} - -/** - * @brief Decode a set_mag_offsets message into a struct - * - * @param msg The message to decode - * @param set_mag_offsets C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_mag_offsets_decode(const mavlink_message_t* msg, mavlink_set_mag_offsets_t* set_mag_offsets) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_mag_offsets->mag_ofs_x = mavlink_msg_set_mag_offsets_get_mag_ofs_x(msg); - set_mag_offsets->mag_ofs_y = mavlink_msg_set_mag_offsets_get_mag_ofs_y(msg); - set_mag_offsets->mag_ofs_z = mavlink_msg_set_mag_offsets_get_mag_ofs_z(msg); - set_mag_offsets->target_system = mavlink_msg_set_mag_offsets_get_target_system(msg); - set_mag_offsets->target_component = mavlink_msg_set_mag_offsets_get_target_component(msg); -#else - memcpy(set_mag_offsets, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h deleted file mode 100644 index 48cfb6ffd..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h +++ /dev/null @@ -1,449 +0,0 @@ -// MESSAGE SIMSTATE PACKING - -#define MAVLINK_MSG_ID_SIMSTATE 164 - -typedef struct __mavlink_simstate_t -{ - float roll; ///< Roll angle (rad) - float pitch; ///< Pitch angle (rad) - float yaw; ///< Yaw angle (rad) - float xacc; ///< X acceleration m/s/s - float yacc; ///< Y acceleration m/s/s - float zacc; ///< Z acceleration m/s/s - float xgyro; ///< Angular speed around X axis rad/s - float ygyro; ///< Angular speed around Y axis rad/s - float zgyro; ///< Angular speed around Z axis rad/s - int32_t lat; ///< Latitude in degrees * 1E7 - int32_t lng; ///< Longitude in degrees * 1E7 -} mavlink_simstate_t; - -#define MAVLINK_MSG_ID_SIMSTATE_LEN 44 -#define MAVLINK_MSG_ID_164_LEN 44 - -#define MAVLINK_MSG_ID_SIMSTATE_CRC 154 -#define MAVLINK_MSG_ID_164_CRC 154 - - - -#define MAVLINK_MESSAGE_INFO_SIMSTATE { \ - "SIMSTATE", \ - 11, \ - { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_simstate_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_simstate_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_simstate_t, yaw) }, \ - { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_simstate_t, xacc) }, \ - { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_simstate_t, yacc) }, \ - { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_simstate_t, zacc) }, \ - { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_simstate_t, xgyro) }, \ - { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_simstate_t, ygyro) }, \ - { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_simstate_t, zgyro) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_simstate_t, lat) }, \ - { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_simstate_t, lng) }, \ - } \ -} - - -/** - * @brief Pack a simstate message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param xacc X acceleration m/s/s - * @param yacc Y acceleration m/s/s - * @param zacc Z acceleration m/s/s - * @param xgyro Angular speed around X axis rad/s - * @param ygyro Angular speed around Y axis rad/s - * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees * 1E7 - * @param lng Longitude in degrees * 1E7 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, xacc); - _mav_put_float(buf, 16, yacc); - _mav_put_float(buf, 20, zacc); - _mav_put_float(buf, 24, xgyro); - _mav_put_float(buf, 28, ygyro); - _mav_put_float(buf, 32, zgyro); - _mav_put_int32_t(buf, 36, lat); - _mav_put_int32_t(buf, 40, lng); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN); -#else - mavlink_simstate_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.lat = lat; - packet.lng = lng; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SIMSTATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif -} - -/** - * @brief Pack a simstate message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param xacc X acceleration m/s/s - * @param yacc Y acceleration m/s/s - * @param zacc Z acceleration m/s/s - * @param xgyro Angular speed around X axis rad/s - * @param ygyro Angular speed around Y axis rad/s - * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees * 1E7 - * @param lng Longitude in degrees * 1E7 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,int32_t lat,int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, xacc); - _mav_put_float(buf, 16, yacc); - _mav_put_float(buf, 20, zacc); - _mav_put_float(buf, 24, xgyro); - _mav_put_float(buf, 28, ygyro); - _mav_put_float(buf, 32, zgyro); - _mav_put_int32_t(buf, 36, lat); - _mav_put_int32_t(buf, 40, lng); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN); -#else - mavlink_simstate_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.lat = lat; - packet.lng = lng; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SIMSTATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif -} - -/** - * @brief Encode a simstate struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param simstate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_simstate_t* simstate) -{ - return mavlink_msg_simstate_pack(system_id, component_id, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng); -} - -/** - * @brief Encode a simstate struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param simstate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_simstate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_simstate_t* simstate) -{ - return mavlink_msg_simstate_pack_chan(system_id, component_id, chan, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng); -} - -/** - * @brief Send a simstate message - * @param chan MAVLink channel to send the message - * - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param xacc X acceleration m/s/s - * @param yacc Y acceleration m/s/s - * @param zacc Z acceleration m/s/s - * @param xgyro Angular speed around X axis rad/s - * @param ygyro Angular speed around Y axis rad/s - * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees * 1E7 - * @param lng Longitude in degrees * 1E7 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, xacc); - _mav_put_float(buf, 16, yacc); - _mav_put_float(buf, 20, zacc); - _mav_put_float(buf, 24, xgyro); - _mav_put_float(buf, 28, ygyro); - _mav_put_float(buf, 32, zgyro); - _mav_put_int32_t(buf, 36, lat); - _mav_put_int32_t(buf, 40, lng); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif -#else - mavlink_simstate_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.lat = lat; - packet.lng = lng; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)&packet, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)&packet, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SIMSTATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_simstate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, xacc); - _mav_put_float(buf, 16, yacc); - _mav_put_float(buf, 20, zacc); - _mav_put_float(buf, 24, xgyro); - _mav_put_float(buf, 28, ygyro); - _mav_put_float(buf, 32, zgyro); - _mav_put_int32_t(buf, 36, lat); - _mav_put_int32_t(buf, 40, lng); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif -#else - mavlink_simstate_t *packet = (mavlink_simstate_t *)msgbuf; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - packet->xacc = xacc; - packet->yacc = yacc; - packet->zacc = zacc; - packet->xgyro = xgyro; - packet->ygyro = ygyro; - packet->zgyro = zgyro; - packet->lat = lat; - packet->lng = lng; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)packet, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)packet, MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SIMSTATE UNPACKING - - -/** - * @brief Get field roll from simstate message - * - * @return Roll angle (rad) - */ -static inline float mavlink_msg_simstate_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field pitch from simstate message - * - * @return Pitch angle (rad) - */ -static inline float mavlink_msg_simstate_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field yaw from simstate message - * - * @return Yaw angle (rad) - */ -static inline float mavlink_msg_simstate_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field xacc from simstate message - * - * @return X acceleration m/s/s - */ -static inline float mavlink_msg_simstate_get_xacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field yacc from simstate message - * - * @return Y acceleration m/s/s - */ -static inline float mavlink_msg_simstate_get_yacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field zacc from simstate message - * - * @return Z acceleration m/s/s - */ -static inline float mavlink_msg_simstate_get_zacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field xgyro from simstate message - * - * @return Angular speed around X axis rad/s - */ -static inline float mavlink_msg_simstate_get_xgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field ygyro from simstate message - * - * @return Angular speed around Y axis rad/s - */ -static inline float mavlink_msg_simstate_get_ygyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field zgyro from simstate message - * - * @return Angular speed around Z axis rad/s - */ -static inline float mavlink_msg_simstate_get_zgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field lat from simstate message - * - * @return Latitude in degrees * 1E7 - */ -static inline int32_t mavlink_msg_simstate_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 36); -} - -/** - * @brief Get field lng from simstate message - * - * @return Longitude in degrees * 1E7 - */ -static inline int32_t mavlink_msg_simstate_get_lng(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 40); -} - -/** - * @brief Decode a simstate message into a struct - * - * @param msg The message to decode - * @param simstate C-struct to decode the message contents into - */ -static inline void mavlink_msg_simstate_decode(const mavlink_message_t* msg, mavlink_simstate_t* simstate) -{ -#if MAVLINK_NEED_BYTE_SWAP - simstate->roll = mavlink_msg_simstate_get_roll(msg); - simstate->pitch = mavlink_msg_simstate_get_pitch(msg); - simstate->yaw = mavlink_msg_simstate_get_yaw(msg); - simstate->xacc = mavlink_msg_simstate_get_xacc(msg); - simstate->yacc = mavlink_msg_simstate_get_yacc(msg); - simstate->zacc = mavlink_msg_simstate_get_zacc(msg); - simstate->xgyro = mavlink_msg_simstate_get_xgyro(msg); - simstate->ygyro = mavlink_msg_simstate_get_ygyro(msg); - simstate->zgyro = mavlink_msg_simstate_get_zgyro(msg); - simstate->lat = mavlink_msg_simstate_get_lat(msg); - simstate->lng = mavlink_msg_simstate_get_lng(msg); -#else - memcpy(simstate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SIMSTATE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h deleted file mode 100644 index 5d5edc4cc..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE WIND PACKING - -#define MAVLINK_MSG_ID_WIND 168 - -typedef struct __mavlink_wind_t -{ - float direction; ///< wind direction that wind is coming from (degrees) - float speed; ///< wind speed in ground plane (m/s) - float speed_z; ///< vertical wind speed (m/s) -} mavlink_wind_t; - -#define MAVLINK_MSG_ID_WIND_LEN 12 -#define MAVLINK_MSG_ID_168_LEN 12 - -#define MAVLINK_MSG_ID_WIND_CRC 1 -#define MAVLINK_MSG_ID_168_CRC 1 - - - -#define MAVLINK_MESSAGE_INFO_WIND { \ - "WIND", \ - 3, \ - { { "direction", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_wind_t, direction) }, \ - { "speed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_wind_t, speed) }, \ - { "speed_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_wind_t, speed_z) }, \ - } \ -} - - -/** - * @brief Pack a wind message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param direction wind direction that wind is coming from (degrees) - * @param speed wind speed in ground plane (m/s) - * @param speed_z vertical wind speed (m/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_wind_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float direction, float speed, float speed_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WIND_LEN]; - _mav_put_float(buf, 0, direction); - _mav_put_float(buf, 4, speed); - _mav_put_float(buf, 8, speed_z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIND_LEN); -#else - mavlink_wind_t packet; - packet.direction = direction; - packet.speed = speed; - packet.speed_z = speed_z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WIND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIND_LEN); -#endif -} - -/** - * @brief Pack a wind message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param direction wind direction that wind is coming from (degrees) - * @param speed wind speed in ground plane (m/s) - * @param speed_z vertical wind speed (m/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_wind_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float direction,float speed,float speed_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WIND_LEN]; - _mav_put_float(buf, 0, direction); - _mav_put_float(buf, 4, speed); - _mav_put_float(buf, 8, speed_z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIND_LEN); -#else - mavlink_wind_t packet; - packet.direction = direction; - packet.speed = speed; - packet.speed_z = speed_z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WIND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WIND_LEN); -#endif -} - -/** - * @brief Encode a wind struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param wind C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_wind_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_wind_t* wind) -{ - return mavlink_msg_wind_pack(system_id, component_id, msg, wind->direction, wind->speed, wind->speed_z); -} - -/** - * @brief Encode a wind struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param wind C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_wind_t* wind) -{ - return mavlink_msg_wind_pack_chan(system_id, component_id, chan, msg, wind->direction, wind->speed, wind->speed_z); -} - -/** - * @brief Send a wind message - * @param chan MAVLink channel to send the message - * - * @param direction wind direction that wind is coming from (degrees) - * @param speed wind speed in ground plane (m/s) - * @param speed_z vertical wind speed (m/s) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_wind_send(mavlink_channel_t chan, float direction, float speed, float speed_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WIND_LEN]; - _mav_put_float(buf, 0, direction); - _mav_put_float(buf, 4, speed); - _mav_put_float(buf, 8, speed_z); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN); -#endif -#else - mavlink_wind_t packet; - packet.direction = direction; - packet.speed = speed; - packet.speed_z = speed_z; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)&packet, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)&packet, MAVLINK_MSG_ID_WIND_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_WIND_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_wind_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float direction, float speed, float speed_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, direction); - _mav_put_float(buf, 4, speed); - _mav_put_float(buf, 8, speed_z); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN); -#endif -#else - mavlink_wind_t *packet = (mavlink_wind_t *)msgbuf; - packet->direction = direction; - packet->speed = speed; - packet->speed_z = speed_z; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)packet, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)packet, MAVLINK_MSG_ID_WIND_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE WIND UNPACKING - - -/** - * @brief Get field direction from wind message - * - * @return wind direction that wind is coming from (degrees) - */ -static inline float mavlink_msg_wind_get_direction(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field speed from wind message - * - * @return wind speed in ground plane (m/s) - */ -static inline float mavlink_msg_wind_get_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field speed_z from wind message - * - * @return vertical wind speed (m/s) - */ -static inline float mavlink_msg_wind_get_speed_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Decode a wind message into a struct - * - * @param msg The message to decode - * @param wind C-struct to decode the message contents into - */ -static inline void mavlink_msg_wind_decode(const mavlink_message_t* msg, mavlink_wind_t* wind) -{ -#if MAVLINK_NEED_BYTE_SWAP - wind->direction = mavlink_msg_wind_get_direction(msg); - wind->speed = mavlink_msg_wind_get_speed(msg); - wind->speed_z = mavlink_msg_wind_get_speed_z(msg); -#else - memcpy(wind, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WIND_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h deleted file mode 100644 index 489553ea4..000000000 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ /dev/null @@ -1,1542 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from ardupilotmega.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef ARDUPILOTMEGA_TESTSUITE_H -#define ARDUPILOTMEGA_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_ardupilotmega(uint8_t, uint8_t, mavlink_message_t *last_msg); - -static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_test_common(system_id, component_id, last_msg); - mavlink_test_ardupilotmega(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_sensor_offsets_t packet_in = { - 17.0, - }963497672, - }963497880, - }101.0, - }129.0, - }157.0, - }185.0, - }213.0, - }241.0, - }19107, - }19211, - }19315, - }; - mavlink_sensor_offsets_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.mag_declination = packet_in.mag_declination; - packet1.raw_press = packet_in.raw_press; - packet1.raw_temp = packet_in.raw_temp; - packet1.gyro_cal_x = packet_in.gyro_cal_x; - packet1.gyro_cal_y = packet_in.gyro_cal_y; - packet1.gyro_cal_z = packet_in.gyro_cal_z; - packet1.accel_cal_x = packet_in.accel_cal_x; - packet1.accel_cal_y = packet_in.accel_cal_y; - packet1.accel_cal_z = packet_in.accel_cal_z; - packet1.mag_ofs_x = packet_in.mag_ofs_x; - packet1.mag_ofs_y = packet_in.mag_ofs_y; - packet1.mag_ofs_z = packet_in.mag_ofs_z; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_sensor_offsets_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_sensor_offsets_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_sensor_offsets_pack(system_id, component_id, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); - mavlink_msg_sensor_offsets_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); - mavlink_msg_sensor_offsets_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; i 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ - MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ - MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ - MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_ENUM_END=501, /* | */ -}; -#endif - -#include "../common/common.h" - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_aq_telemetry_f.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // AUTOQUAD_H diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink.h deleted file mode 100644 index 3f80c9a41..000000000 --- a/mavlink/include/mavlink/v1.0/autoquad/mavlink.h +++ /dev/null @@ -1,27 +0,0 @@ -/** @file - * @brief MAVLink comm protocol built from autoquad.xml - * @see http://pixhawk.ethz.ch/software/mavlink - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#ifndef MAVLINK_STX -#define MAVLINK_STX 254 -#endif - -#ifndef MAVLINK_ENDIAN -#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN -#endif - -#ifndef MAVLINK_ALIGNED_FIELDS -#define MAVLINK_ALIGNED_FIELDS 1 -#endif - -#ifndef MAVLINK_CRC_EXTRA -#define MAVLINK_CRC_EXTRA 1 -#endif - -#include "version.h" -#include "autoquad.h" - -#endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h deleted file mode 100644 index e78d5687a..000000000 --- a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h +++ /dev/null @@ -1,689 +0,0 @@ -// MESSAGE AQ_TELEMETRY_F PACKING - -#define MAVLINK_MSG_ID_AQ_TELEMETRY_F 150 - -typedef struct __mavlink_aq_telemetry_f_t -{ - float value1; ///< value1 - float value2; ///< value2 - float value3; ///< value3 - float value4; ///< value4 - float value5; ///< value5 - float value6; ///< value6 - float value7; ///< value7 - float value8; ///< value8 - float value9; ///< value9 - float value10; ///< value10 - float value11; ///< value11 - float value12; ///< value12 - float value13; ///< value13 - float value14; ///< value14 - float value15; ///< value15 - float value16; ///< value16 - float value17; ///< value17 - float value18; ///< value18 - float value19; ///< value19 - float value20; ///< value20 - uint16_t Index; ///< Index of message -} mavlink_aq_telemetry_f_t; - -#define MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN 82 -#define MAVLINK_MSG_ID_150_LEN 82 - -#define MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC 241 -#define MAVLINK_MSG_ID_150_CRC 241 - - - -#define MAVLINK_MESSAGE_INFO_AQ_TELEMETRY_F { \ - "AQ_TELEMETRY_F", \ - 21, \ - { { "value1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_aq_telemetry_f_t, value1) }, \ - { "value2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_aq_telemetry_f_t, value2) }, \ - { "value3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_aq_telemetry_f_t, value3) }, \ - { "value4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_aq_telemetry_f_t, value4) }, \ - { "value5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_aq_telemetry_f_t, value5) }, \ - { "value6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_aq_telemetry_f_t, value6) }, \ - { "value7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_aq_telemetry_f_t, value7) }, \ - { "value8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_aq_telemetry_f_t, value8) }, \ - { "value9", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_aq_telemetry_f_t, value9) }, \ - { "value10", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_aq_telemetry_f_t, value10) }, \ - { "value11", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_aq_telemetry_f_t, value11) }, \ - { "value12", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_aq_telemetry_f_t, value12) }, \ - { "value13", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_aq_telemetry_f_t, value13) }, \ - { "value14", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_aq_telemetry_f_t, value14) }, \ - { "value15", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_aq_telemetry_f_t, value15) }, \ - { "value16", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_aq_telemetry_f_t, value16) }, \ - { "value17", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_aq_telemetry_f_t, value17) }, \ - { "value18", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_aq_telemetry_f_t, value18) }, \ - { "value19", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_aq_telemetry_f_t, value19) }, \ - { "value20", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_aq_telemetry_f_t, value20) }, \ - { "Index", NULL, MAVLINK_TYPE_UINT16_T, 0, 80, offsetof(mavlink_aq_telemetry_f_t, Index) }, \ - } \ -} - - -/** - * @brief Pack a aq_telemetry_f message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param Index Index of message - * @param value1 value1 - * @param value2 value2 - * @param value3 value3 - * @param value4 value4 - * @param value5 value5 - * @param value6 value6 - * @param value7 value7 - * @param value8 value8 - * @param value9 value9 - * @param value10 value10 - * @param value11 value11 - * @param value12 value12 - * @param value13 value13 - * @param value14 value14 - * @param value15 value15 - * @param value16 value16 - * @param value17 value17 - * @param value18 value18 - * @param value19 value19 - * @param value20 value20 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t Index, float value1, float value2, float value3, float value4, float value5, float value6, float value7, float value8, float value9, float value10, float value11, float value12, float value13, float value14, float value15, float value16, float value17, float value18, float value19, float value20) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN]; - _mav_put_float(buf, 0, value1); - _mav_put_float(buf, 4, value2); - _mav_put_float(buf, 8, value3); - _mav_put_float(buf, 12, value4); - _mav_put_float(buf, 16, value5); - _mav_put_float(buf, 20, value6); - _mav_put_float(buf, 24, value7); - _mav_put_float(buf, 28, value8); - _mav_put_float(buf, 32, value9); - _mav_put_float(buf, 36, value10); - _mav_put_float(buf, 40, value11); - _mav_put_float(buf, 44, value12); - _mav_put_float(buf, 48, value13); - _mav_put_float(buf, 52, value14); - _mav_put_float(buf, 56, value15); - _mav_put_float(buf, 60, value16); - _mav_put_float(buf, 64, value17); - _mav_put_float(buf, 68, value18); - _mav_put_float(buf, 72, value19); - _mav_put_float(buf, 76, value20); - _mav_put_uint16_t(buf, 80, Index); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#else - mavlink_aq_telemetry_f_t packet; - packet.value1 = value1; - packet.value2 = value2; - packet.value3 = value3; - packet.value4 = value4; - packet.value5 = value5; - packet.value6 = value6; - packet.value7 = value7; - packet.value8 = value8; - packet.value9 = value9; - packet.value10 = value10; - packet.value11 = value11; - packet.value12 = value12; - packet.value13 = value13; - packet.value14 = value14; - packet.value15 = value15; - packet.value16 = value16; - packet.value17 = value17; - packet.value18 = value18; - packet.value19 = value19; - packet.value20 = value20; - packet.Index = Index; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AQ_TELEMETRY_F; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -} - -/** - * @brief Pack a aq_telemetry_f message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param Index Index of message - * @param value1 value1 - * @param value2 value2 - * @param value3 value3 - * @param value4 value4 - * @param value5 value5 - * @param value6 value6 - * @param value7 value7 - * @param value8 value8 - * @param value9 value9 - * @param value10 value10 - * @param value11 value11 - * @param value12 value12 - * @param value13 value13 - * @param value14 value14 - * @param value15 value15 - * @param value16 value16 - * @param value17 value17 - * @param value18 value18 - * @param value19 value19 - * @param value20 value20 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t Index,float value1,float value2,float value3,float value4,float value5,float value6,float value7,float value8,float value9,float value10,float value11,float value12,float value13,float value14,float value15,float value16,float value17,float value18,float value19,float value20) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN]; - _mav_put_float(buf, 0, value1); - _mav_put_float(buf, 4, value2); - _mav_put_float(buf, 8, value3); - _mav_put_float(buf, 12, value4); - _mav_put_float(buf, 16, value5); - _mav_put_float(buf, 20, value6); - _mav_put_float(buf, 24, value7); - _mav_put_float(buf, 28, value8); - _mav_put_float(buf, 32, value9); - _mav_put_float(buf, 36, value10); - _mav_put_float(buf, 40, value11); - _mav_put_float(buf, 44, value12); - _mav_put_float(buf, 48, value13); - _mav_put_float(buf, 52, value14); - _mav_put_float(buf, 56, value15); - _mav_put_float(buf, 60, value16); - _mav_put_float(buf, 64, value17); - _mav_put_float(buf, 68, value18); - _mav_put_float(buf, 72, value19); - _mav_put_float(buf, 76, value20); - _mav_put_uint16_t(buf, 80, Index); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#else - mavlink_aq_telemetry_f_t packet; - packet.value1 = value1; - packet.value2 = value2; - packet.value3 = value3; - packet.value4 = value4; - packet.value5 = value5; - packet.value6 = value6; - packet.value7 = value7; - packet.value8 = value8; - packet.value9 = value9; - packet.value10 = value10; - packet.value11 = value11; - packet.value12 = value12; - packet.value13 = value13; - packet.value14 = value14; - packet.value15 = value15; - packet.value16 = value16; - packet.value17 = value17; - packet.value18 = value18; - packet.value19 = value19; - packet.value20 = value20; - packet.Index = Index; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AQ_TELEMETRY_F; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -} - -/** - * @brief Encode a aq_telemetry_f struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param aq_telemetry_f C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f) -{ - return mavlink_msg_aq_telemetry_f_pack(system_id, component_id, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20); -} - -/** - * @brief Encode a aq_telemetry_f struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param aq_telemetry_f C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f) -{ - return mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, chan, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20); -} - -/** - * @brief Send a aq_telemetry_f message - * @param chan MAVLink channel to send the message - * - * @param Index Index of message - * @param value1 value1 - * @param value2 value2 - * @param value3 value3 - * @param value4 value4 - * @param value5 value5 - * @param value6 value6 - * @param value7 value7 - * @param value8 value8 - * @param value9 value9 - * @param value10 value10 - * @param value11 value11 - * @param value12 value12 - * @param value13 value13 - * @param value14 value14 - * @param value15 value15 - * @param value16 value16 - * @param value17 value17 - * @param value18 value18 - * @param value19 value19 - * @param value20 value20 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_aq_telemetry_f_send(mavlink_channel_t chan, uint16_t Index, float value1, float value2, float value3, float value4, float value5, float value6, float value7, float value8, float value9, float value10, float value11, float value12, float value13, float value14, float value15, float value16, float value17, float value18, float value19, float value20) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN]; - _mav_put_float(buf, 0, value1); - _mav_put_float(buf, 4, value2); - _mav_put_float(buf, 8, value3); - _mav_put_float(buf, 12, value4); - _mav_put_float(buf, 16, value5); - _mav_put_float(buf, 20, value6); - _mav_put_float(buf, 24, value7); - _mav_put_float(buf, 28, value8); - _mav_put_float(buf, 32, value9); - _mav_put_float(buf, 36, value10); - _mav_put_float(buf, 40, value11); - _mav_put_float(buf, 44, value12); - _mav_put_float(buf, 48, value13); - _mav_put_float(buf, 52, value14); - _mav_put_float(buf, 56, value15); - _mav_put_float(buf, 60, value16); - _mav_put_float(buf, 64, value17); - _mav_put_float(buf, 68, value18); - _mav_put_float(buf, 72, value19); - _mav_put_float(buf, 76, value20); - _mav_put_uint16_t(buf, 80, Index); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -#else - mavlink_aq_telemetry_f_t packet; - packet.value1 = value1; - packet.value2 = value2; - packet.value3 = value3; - packet.value4 = value4; - packet.value5 = value5; - packet.value6 = value6; - packet.value7 = value7; - packet.value8 = value8; - packet.value9 = value9; - packet.value10 = value10; - packet.value11 = value11; - packet.value12 = value12; - packet.value13 = value13; - packet.value14 = value14; - packet.value15 = value15; - packet.value16 = value16; - packet.value17 = value17; - packet.value18 = value18; - packet.value19 = value19; - packet.value20 = value20; - packet.Index = Index; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)&packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)&packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_aq_telemetry_f_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Index, float value1, float value2, float value3, float value4, float value5, float value6, float value7, float value8, float value9, float value10, float value11, float value12, float value13, float value14, float value15, float value16, float value17, float value18, float value19, float value20) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, value1); - _mav_put_float(buf, 4, value2); - _mav_put_float(buf, 8, value3); - _mav_put_float(buf, 12, value4); - _mav_put_float(buf, 16, value5); - _mav_put_float(buf, 20, value6); - _mav_put_float(buf, 24, value7); - _mav_put_float(buf, 28, value8); - _mav_put_float(buf, 32, value9); - _mav_put_float(buf, 36, value10); - _mav_put_float(buf, 40, value11); - _mav_put_float(buf, 44, value12); - _mav_put_float(buf, 48, value13); - _mav_put_float(buf, 52, value14); - _mav_put_float(buf, 56, value15); - _mav_put_float(buf, 60, value16); - _mav_put_float(buf, 64, value17); - _mav_put_float(buf, 68, value18); - _mav_put_float(buf, 72, value19); - _mav_put_float(buf, 76, value20); - _mav_put_uint16_t(buf, 80, Index); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -#else - mavlink_aq_telemetry_f_t *packet = (mavlink_aq_telemetry_f_t *)msgbuf; - packet->value1 = value1; - packet->value2 = value2; - packet->value3 = value3; - packet->value4 = value4; - packet->value5 = value5; - packet->value6 = value6; - packet->value7 = value7; - packet->value8 = value8; - packet->value9 = value9; - packet->value10 = value10; - packet->value11 = value11; - packet->value12 = value12; - packet->value13 = value13; - packet->value14 = value14; - packet->value15 = value15; - packet->value16 = value16; - packet->value17 = value17; - packet->value18 = value18; - packet->value19 = value19; - packet->value20 = value20; - packet->Index = Index; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE AQ_TELEMETRY_F UNPACKING - - -/** - * @brief Get field Index from aq_telemetry_f message - * - * @return Index of message - */ -static inline uint16_t mavlink_msg_aq_telemetry_f_get_Index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 80); -} - -/** - * @brief Get field value1 from aq_telemetry_f message - * - * @return value1 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field value2 from aq_telemetry_f message - * - * @return value2 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field value3 from aq_telemetry_f message - * - * @return value3 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field value4 from aq_telemetry_f message - * - * @return value4 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field value5 from aq_telemetry_f message - * - * @return value5 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field value6 from aq_telemetry_f message - * - * @return value6 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field value7 from aq_telemetry_f message - * - * @return value7 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field value8 from aq_telemetry_f message - * - * @return value8 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field value9 from aq_telemetry_f message - * - * @return value9 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value9(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field value10 from aq_telemetry_f message - * - * @return value10 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value10(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 36); -} - -/** - * @brief Get field value11 from aq_telemetry_f message - * - * @return value11 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value11(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 40); -} - -/** - * @brief Get field value12 from aq_telemetry_f message - * - * @return value12 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value12(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 44); -} - -/** - * @brief Get field value13 from aq_telemetry_f message - * - * @return value13 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value13(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 48); -} - -/** - * @brief Get field value14 from aq_telemetry_f message - * - * @return value14 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value14(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 52); -} - -/** - * @brief Get field value15 from aq_telemetry_f message - * - * @return value15 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value15(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 56); -} - -/** - * @brief Get field value16 from aq_telemetry_f message - * - * @return value16 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value16(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 60); -} - -/** - * @brief Get field value17 from aq_telemetry_f message - * - * @return value17 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value17(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 64); -} - -/** - * @brief Get field value18 from aq_telemetry_f message - * - * @return value18 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value18(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 68); -} - -/** - * @brief Get field value19 from aq_telemetry_f message - * - * @return value19 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value19(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 72); -} - -/** - * @brief Get field value20 from aq_telemetry_f message - * - * @return value20 - */ -static inline float mavlink_msg_aq_telemetry_f_get_value20(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 76); -} - -/** - * @brief Decode a aq_telemetry_f message into a struct - * - * @param msg The message to decode - * @param aq_telemetry_f C-struct to decode the message contents into - */ -static inline void mavlink_msg_aq_telemetry_f_decode(const mavlink_message_t* msg, mavlink_aq_telemetry_f_t* aq_telemetry_f) -{ -#if MAVLINK_NEED_BYTE_SWAP - aq_telemetry_f->value1 = mavlink_msg_aq_telemetry_f_get_value1(msg); - aq_telemetry_f->value2 = mavlink_msg_aq_telemetry_f_get_value2(msg); - aq_telemetry_f->value3 = mavlink_msg_aq_telemetry_f_get_value3(msg); - aq_telemetry_f->value4 = mavlink_msg_aq_telemetry_f_get_value4(msg); - aq_telemetry_f->value5 = mavlink_msg_aq_telemetry_f_get_value5(msg); - aq_telemetry_f->value6 = mavlink_msg_aq_telemetry_f_get_value6(msg); - aq_telemetry_f->value7 = mavlink_msg_aq_telemetry_f_get_value7(msg); - aq_telemetry_f->value8 = mavlink_msg_aq_telemetry_f_get_value8(msg); - aq_telemetry_f->value9 = mavlink_msg_aq_telemetry_f_get_value9(msg); - aq_telemetry_f->value10 = mavlink_msg_aq_telemetry_f_get_value10(msg); - aq_telemetry_f->value11 = mavlink_msg_aq_telemetry_f_get_value11(msg); - aq_telemetry_f->value12 = mavlink_msg_aq_telemetry_f_get_value12(msg); - aq_telemetry_f->value13 = mavlink_msg_aq_telemetry_f_get_value13(msg); - aq_telemetry_f->value14 = mavlink_msg_aq_telemetry_f_get_value14(msg); - aq_telemetry_f->value15 = mavlink_msg_aq_telemetry_f_get_value15(msg); - aq_telemetry_f->value16 = mavlink_msg_aq_telemetry_f_get_value16(msg); - aq_telemetry_f->value17 = mavlink_msg_aq_telemetry_f_get_value17(msg); - aq_telemetry_f->value18 = mavlink_msg_aq_telemetry_f_get_value18(msg); - aq_telemetry_f->value19 = mavlink_msg_aq_telemetry_f_get_value19(msg); - aq_telemetry_f->value20 = mavlink_msg_aq_telemetry_f_get_value20(msg); - aq_telemetry_f->Index = mavlink_msg_aq_telemetry_f_get_Index(msg); -#else - memcpy(aq_telemetry_f, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/autoquad/testsuite.h b/mavlink/include/mavlink/v1.0/autoquad/testsuite.h deleted file mode 100644 index 4eafe7be5..000000000 --- a/mavlink/include/mavlink/v1.0/autoquad/testsuite.h +++ /dev/null @@ -1,118 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from autoquad.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef AUTOQUAD_TESTSUITE_H -#define AUTOQUAD_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_autoquad(uint8_t, uint8_t, mavlink_message_t *last_msg); - -static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_test_common(system_id, component_id, last_msg); - mavlink_test_autoquad(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_aq_telemetry_f(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_aq_telemetry_f_t packet_in = { - 17.0, - }45.0, - }73.0, - }101.0, - }129.0, - }157.0, - }185.0, - }213.0, - }241.0, - }269.0, - }297.0, - }325.0, - }353.0, - }381.0, - }409.0, - }437.0, - }465.0, - }493.0, - }521.0, - }549.0, - }21395, - }; - mavlink_aq_telemetry_f_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.value1 = packet_in.value1; - packet1.value2 = packet_in.value2; - packet1.value3 = packet_in.value3; - packet1.value4 = packet_in.value4; - packet1.value5 = packet_in.value5; - packet1.value6 = packet_in.value6; - packet1.value7 = packet_in.value7; - packet1.value8 = packet_in.value8; - packet1.value9 = packet_in.value9; - packet1.value10 = packet_in.value10; - packet1.value11 = packet_in.value11; - packet1.value12 = packet_in.value12; - packet1.value13 = packet_in.value13; - packet1.value14 = packet_in.value14; - packet1.value15 = packet_in.value15; - packet1.value16 = packet_in.value16; - packet1.value17 = packet_in.value17; - packet1.value18 = packet_in.value18; - packet1.value19 = packet_in.value19; - packet1.value20 = packet_in.value20; - packet1.Index = packet_in.Index; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_aq_telemetry_f_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_aq_telemetry_f_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_aq_telemetry_f_pack(system_id, component_id, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 ); - mavlink_msg_aq_telemetry_f_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 ); - mavlink_msg_aq_telemetry_f_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; i>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); -} -#endif - -/** - * @brief Initiliaze the buffer for the X.25 CRC - * - * @param crcAccum the 16 bit X.25 CRC - */ -static inline void crc_init(uint16_t* crcAccum) -{ - *crcAccum = X25_INIT_CRC; -} - - -/** - * @brief Calculates the X.25 checksum on a byte buffer - * - * @param pBuffer buffer containing the byte array to hash - * @param length length of the byte array - * @return the checksum over the buffer bytes - **/ -static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) -{ - uint16_t crcTmp; - crc_init(&crcTmp); - while (length--) { - crc_accumulate(*pBuffer++, &crcTmp); - } - return crcTmp; -} - -/** - * @brief Accumulate the X.25 CRC by adding an array of bytes - * - * The checksum function adds the hash of one char at a time to the - * 16 bit checksum (uint16_t). - * - * @param data new bytes to hash - * @param crcAccum the already accumulated checksum - **/ -static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length) -{ - const uint8_t *p = (const uint8_t *)pBuffer; - while (length--) { - crc_accumulate(*p++, crcAccum); - } -} - - - - -#endif /* _CHECKSUM_H_ */ - -#ifdef __cplusplus -} -#endif diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h deleted file mode 100644 index de6e22011..000000000 --- a/mavlink/include/mavlink/v1.0/common/common.h +++ /dev/null @@ -1,670 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from common.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef COMMON_H -#define COMMON_H - -#ifndef MAVLINK_H - #error Wrong include order: common.h MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set all defines from mavlink.h manually. -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_COMMON - -// ENUM DEFINITIONS - - -/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ -#ifndef HAVE_ENUM_MAV_AUTOPILOT -#define HAVE_ENUM_MAV_AUTOPILOT -enum MAV_AUTOPILOT -{ - MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */ - MAV_AUTOPILOT_PIXHAWK=1, /* PIXHAWK autopilot, http://pixhawk.ethz.ch | */ - MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */ - MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilotMega / ArduCopter, http://diydrones.com | */ - MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */ - MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */ - MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */ - MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */ - MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ - MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ - MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */ - MAV_AUTOPILOT_FP=11, /* FlexiPilot | */ - MAV_AUTOPILOT_PX4=12, /* PX4 Autopilot - http://pixhawk.ethz.ch/px4/ | */ - MAV_AUTOPILOT_SMACCMPILOT=13, /* SMACCMPilot - http://smaccmpilot.org | */ - MAV_AUTOPILOT_AUTOQUAD=14, /* AutoQuad -- http://autoquad.org | */ - MAV_AUTOPILOT_ARMAZILA=15, /* Armazila -- http://armazila.com | */ - MAV_AUTOPILOT_AEROB=16, /* Aerob -- http://aerob.ru | */ - MAV_AUTOPILOT_ENUM_END=17, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_TYPE -#define HAVE_ENUM_MAV_TYPE -enum MAV_TYPE -{ - MAV_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ - MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ - MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ - MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */ - MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ - MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ - MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */ - MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */ - MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ - MAV_TYPE_ROCKET=9, /* Rocket | */ - MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */ - MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ - MAV_TYPE_SUBMARINE=12, /* Submarine | */ - MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ - MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ - MAV_TYPE_TRICOPTER=15, /* Octorotor | */ - MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ - MAV_TYPE_KITE=17, /* Flapping wing | */ - MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ - MAV_TYPE_ENUM_END=19, /* | */ -}; -#endif - -/** @brief These flags encode the MAV mode. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG -#define HAVE_ENUM_MAV_MODE_FLAG -enum MAV_MODE_FLAG -{ - MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */ - MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ - MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ - MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ - MAV_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ - MAV_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ - MAV_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */ - MAV_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ - MAV_MODE_FLAG_ENUM_END=129, /* | */ -}; -#endif - -/** @brief These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION -#define HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION -enum MAV_MODE_FLAG_DECODE_POSITION -{ - MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE=1, /* Eighth bit: 00000001 | */ - MAV_MODE_FLAG_DECODE_POSITION_TEST=2, /* Seventh bit: 00000010 | */ - MAV_MODE_FLAG_DECODE_POSITION_AUTO=4, /* Sixt bit: 00000100 | */ - MAV_MODE_FLAG_DECODE_POSITION_GUIDED=8, /* Fifth bit: 00001000 | */ - MAV_MODE_FLAG_DECODE_POSITION_STABILIZE=16, /* Fourth bit: 00010000 | */ - MAV_MODE_FLAG_DECODE_POSITION_HIL=32, /* Third bit: 00100000 | */ - MAV_MODE_FLAG_DECODE_POSITION_MANUAL=64, /* Second bit: 01000000 | */ - MAV_MODE_FLAG_DECODE_POSITION_SAFETY=128, /* First bit: 10000000 | */ - MAV_MODE_FLAG_DECODE_POSITION_ENUM_END=129, /* | */ -}; -#endif - -/** @brief Override command, pauses current mission execution and moves immediately to a position */ -#ifndef HAVE_ENUM_MAV_GOTO -#define HAVE_ENUM_MAV_GOTO -enum MAV_GOTO -{ - MAV_GOTO_DO_HOLD=0, /* Hold at the current position. | */ - MAV_GOTO_DO_CONTINUE=1, /* Continue with the next item in mission execution. | */ - MAV_GOTO_HOLD_AT_CURRENT_POSITION=2, /* Hold at the current position of the system | */ - MAV_GOTO_HOLD_AT_SPECIFIED_POSITION=3, /* Hold at the position specified in the parameters of the DO_HOLD action | */ - MAV_GOTO_ENUM_END=4, /* | */ -}; -#endif - -/** @brief These defines are predefined OR-combined mode flags. There is no need to use values from this enum, but it - simplifies the use of the mode flags. Note that manual input is enabled in all modes as a safety override. */ -#ifndef HAVE_ENUM_MAV_MODE -#define HAVE_ENUM_MAV_MODE -enum MAV_MODE -{ - MAV_MODE_PREFLIGHT=0, /* System is not ready to fly, booting, calibrating, etc. No flag is set. | */ - MAV_MODE_MANUAL_DISARMED=64, /* System is allowed to be active, under manual (RC) control, no stabilization | */ - MAV_MODE_TEST_DISARMED=66, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */ - MAV_MODE_STABILIZE_DISARMED=80, /* System is allowed to be active, under assisted RC control. | */ - MAV_MODE_GUIDED_DISARMED=88, /* System is allowed to be active, under autonomous control, manual setpoint | */ - MAV_MODE_AUTO_DISARMED=92, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by MISSIONs) | */ - MAV_MODE_MANUAL_ARMED=192, /* System is allowed to be active, under manual (RC) control, no stabilization | */ - MAV_MODE_TEST_ARMED=194, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */ - MAV_MODE_STABILIZE_ARMED=208, /* System is allowed to be active, under assisted RC control. | */ - MAV_MODE_GUIDED_ARMED=216, /* System is allowed to be active, under autonomous control, manual setpoint | */ - MAV_MODE_AUTO_ARMED=220, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by MISSIONs) | */ - MAV_MODE_ENUM_END=221, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_STATE -#define HAVE_ENUM_MAV_STATE -enum MAV_STATE -{ - MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */ - MAV_STATE_BOOT=1, /* System is booting up. | */ - MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ - MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ - MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ - MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ - MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ - MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ - MAV_STATE_ENUM_END=8, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_COMPONENT -#define HAVE_ENUM_MAV_COMPONENT -enum MAV_COMPONENT -{ - MAV_COMP_ID_ALL=0, /* | */ - MAV_COMP_ID_CAMERA=100, /* | */ - MAV_COMP_ID_SERVO1=140, /* | */ - MAV_COMP_ID_SERVO2=141, /* | */ - MAV_COMP_ID_SERVO3=142, /* | */ - MAV_COMP_ID_SERVO4=143, /* | */ - MAV_COMP_ID_SERVO5=144, /* | */ - MAV_COMP_ID_SERVO6=145, /* | */ - MAV_COMP_ID_SERVO7=146, /* | */ - MAV_COMP_ID_SERVO8=147, /* | */ - MAV_COMP_ID_SERVO9=148, /* | */ - MAV_COMP_ID_SERVO10=149, /* | */ - MAV_COMP_ID_SERVO11=150, /* | */ - MAV_COMP_ID_SERVO12=151, /* | */ - MAV_COMP_ID_SERVO13=152, /* | */ - MAV_COMP_ID_SERVO14=153, /* | */ - MAV_COMP_ID_MAPPER=180, /* | */ - MAV_COMP_ID_MISSIONPLANNER=190, /* | */ - MAV_COMP_ID_PATHPLANNER=195, /* | */ - MAV_COMP_ID_IMU=200, /* | */ - MAV_COMP_ID_IMU_2=201, /* | */ - MAV_COMP_ID_IMU_3=202, /* | */ - MAV_COMP_ID_GPS=220, /* | */ - MAV_COMP_ID_UDP_BRIDGE=240, /* | */ - MAV_COMP_ID_UART_BRIDGE=241, /* | */ - MAV_COMP_ID_SYSTEM_CONTROL=250, /* | */ - MAV_COMPONENT_ENUM_END=251, /* | */ -}; -#endif - -/** @brief These encode the sensors whose status is sent as part of the SYS_STATUS message. */ -#ifndef HAVE_ENUM_MAV_SYS_STATUS_SENSOR -#define HAVE_ENUM_MAV_SYS_STATUS_SENSOR -enum MAV_SYS_STATUS_SENSOR -{ - MAV_SYS_STATUS_SENSOR_3D_GYRO=1, /* 0x01 3D gyro | */ - MAV_SYS_STATUS_SENSOR_3D_ACCEL=2, /* 0x02 3D accelerometer | */ - MAV_SYS_STATUS_SENSOR_3D_MAG=4, /* 0x04 3D magnetometer | */ - MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE=8, /* 0x08 absolute pressure | */ - MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE=16, /* 0x10 differential pressure | */ - MAV_SYS_STATUS_SENSOR_GPS=32, /* 0x20 GPS | */ - MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW=64, /* 0x40 optical flow | */ - MAV_SYS_STATUS_SENSOR_VISION_POSITION=128, /* 0x80 computer vision position | */ - MAV_SYS_STATUS_SENSOR_LASER_POSITION=256, /* 0x100 laser based position | */ - MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH=512, /* 0x200 external ground truth (Vicon or Leica) | */ - MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL=1024, /* 0x400 3D angular rate control | */ - MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION=2048, /* 0x800 attitude stabilization | */ - MAV_SYS_STATUS_SENSOR_YAW_POSITION=4096, /* 0x1000 yaw position | */ - MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL=8192, /* 0x2000 z/altitude control | */ - MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL=16384, /* 0x4000 x/y position control | */ - MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS=32768, /* 0x8000 motor outputs / control | */ - MAV_SYS_STATUS_SENSOR_RC_RECEIVER=65536, /* 0x10000 rc receiver | */ - MAV_SYS_STATUS_SENSOR_3D_GYRO2=131072, /* 0x20000 2nd 3D gyro | */ - MAV_SYS_STATUS_SENSOR_3D_ACCEL2=262144, /* 0x40000 2nd 3D accelerometer | */ - MAV_SYS_STATUS_SENSOR_3D_MAG2=524288, /* 0x80000 2nd 3D magnetometer | */ - MAV_SYS_STATUS_GEOFENCE=1048576, /* 0x100000 geofence | */ - MAV_SYS_STATUS_SENSOR_ENUM_END=1048577, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_FRAME -#define HAVE_ENUM_MAV_FRAME -enum MAV_FRAME -{ - MAV_FRAME_GLOBAL=0, /* Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL) | */ - MAV_FRAME_LOCAL_NED=1, /* Local coordinate frame, Z-up (x: north, y: east, z: down). | */ - MAV_FRAME_MISSION=2, /* NOT a coordinate frame, indicates a mission command. | */ - MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. | */ - MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-down (x: east, y: north, z: up) | */ - MAV_FRAME_GLOBAL_INT=5, /* Global coordinate frame with some fields as scaled integers, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL). Lat / Lon are scaled * 1E7 to avoid floating point accuracy limitations. | */ - MAV_FRAME_GLOBAL_RELATIVE_ALT_INT=6, /* Global coordinate frame with some fields as scaled integers, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. Lat / Lon are scaled * 1E7 to avoid floating point accuracy limitations. | */ - MAV_FRAME_ENUM_END=7, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE -#define HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE -enum MAVLINK_DATA_STREAM_TYPE -{ - MAVLINK_DATA_STREAM_IMG_JPEG=1, /* | */ - MAVLINK_DATA_STREAM_IMG_BMP=2, /* | */ - MAVLINK_DATA_STREAM_IMG_RAW8U=3, /* | */ - MAVLINK_DATA_STREAM_IMG_RAW32U=4, /* | */ - MAVLINK_DATA_STREAM_IMG_PGM=5, /* | */ - MAVLINK_DATA_STREAM_IMG_PNG=6, /* | */ - MAVLINK_DATA_STREAM_TYPE_ENUM_END=7, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_FENCE_ACTION -#define HAVE_ENUM_FENCE_ACTION -enum FENCE_ACTION -{ - FENCE_ACTION_NONE=0, /* Disable fenced mode | */ - FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */ - FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */ - FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */ - FENCE_ACTION_ENUM_END=4, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_FENCE_BREACH -#define HAVE_ENUM_FENCE_BREACH -enum FENCE_BREACH -{ - FENCE_BREACH_NONE=0, /* No last fence breach | */ - FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */ - FENCE_BREACH_MAXALT=2, /* Breached maximum altitude | */ - FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */ - FENCE_BREACH_ENUM_END=4, /* | */ -}; -#endif - -/** @brief Enumeration of possible mount operation modes */ -#ifndef HAVE_ENUM_MAV_MOUNT_MODE -#define HAVE_ENUM_MAV_MOUNT_MODE -enum MAV_MOUNT_MODE -{ - MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization | */ - MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | */ - MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ - MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ - MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ - MAV_MOUNT_MODE_ENUM_END=5, /* | */ -}; -#endif - -/** @brief Enumeration of distance sensor types */ -#ifndef HAVE_ENUM_MAV_DISTANCE_SENSOR -#define HAVE_ENUM_MAV_DISTANCE_SENSOR -enum MAV_DISTANCE_SENSOR -{ - MAV_DISTANCE_SENSOR_LASER=0, /* Laser altimeter, e.g. LightWare SF02/F or PulsedLight units | */ - MAV_DISTANCE_SENSOR_ULTRASOUND=1, /* Laser altimeter, e.g. MaxBotix units | */ - MAV_DISTANCE_SENSOR_ENUM_END=2, /* | */ -}; -#endif - -/** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */ -#ifndef HAVE_ENUM_MAV_CMD -#define HAVE_ENUM_MAV_CMD -enum MAV_CMD -{ - MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ - MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ - MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ - MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_ENUM_END=501, /* | */ -}; -#endif - -/** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a - recommendation to the autopilot software. Individual autopilots may or may not obey - the recommended messages. */ -#ifndef HAVE_ENUM_MAV_DATA_STREAM -#define HAVE_ENUM_MAV_DATA_STREAM -enum MAV_DATA_STREAM -{ - MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ - MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */ - MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */ - MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */ - MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */ - MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */ - MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */ - MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ - MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ - MAV_DATA_STREAM_ENUM_END=13, /* | */ -}; -#endif - -/** @brief The ROI (region of interest) for the vehicle. This can be - be used by the vehicle for camera/vehicle attitude alignment (see - MAV_CMD_NAV_ROI). */ -#ifndef HAVE_ENUM_MAV_ROI -#define HAVE_ENUM_MAV_ROI -enum MAV_ROI -{ - MAV_ROI_NONE=0, /* No region of interest. | */ - MAV_ROI_WPNEXT=1, /* Point toward next MISSION. | */ - MAV_ROI_WPINDEX=2, /* Point toward given MISSION. | */ - MAV_ROI_LOCATION=3, /* Point toward fixed location. | */ - MAV_ROI_TARGET=4, /* Point toward of given id. | */ - MAV_ROI_ENUM_END=5, /* | */ -}; -#endif - -/** @brief ACK / NACK / ERROR values as a result of MAV_CMDs and for mission item transmission. */ -#ifndef HAVE_ENUM_MAV_CMD_ACK -#define HAVE_ENUM_MAV_CMD_ACK -enum MAV_CMD_ACK -{ - MAV_CMD_ACK_OK=1, /* Command / mission item is ok. | */ - MAV_CMD_ACK_ERR_FAIL=2, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. | */ - MAV_CMD_ACK_ERR_ACCESS_DENIED=3, /* The system is refusing to accept this command from this source / communication partner. | */ - MAV_CMD_ACK_ERR_NOT_SUPPORTED=4, /* Command or mission item is not supported, other commands would be accepted. | */ - MAV_CMD_ACK_ERR_COORDINATE_FRAME_NOT_SUPPORTED=5, /* The coordinate frame of this command / mission item is not supported. | */ - MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE=6, /* The coordinate frame of this command is ok, but he coordinate values exceed the safety limits of this system. This is a generic error, please use the more specific error messages below if possible. | */ - MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE=7, /* The X or latitude value is out of range. | */ - MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=8, /* The Y or longitude value is out of range. | */ - MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=9, /* The Z or altitude value is out of range. | */ - MAV_CMD_ACK_ENUM_END=10, /* | */ -}; -#endif - -/** @brief Specifies the datatype of a MAVLink parameter. */ -#ifndef HAVE_ENUM_MAV_PARAM_TYPE -#define HAVE_ENUM_MAV_PARAM_TYPE -enum MAV_PARAM_TYPE -{ - MAV_PARAM_TYPE_UINT8=1, /* 8-bit unsigned integer | */ - MAV_PARAM_TYPE_INT8=2, /* 8-bit signed integer | */ - MAV_PARAM_TYPE_UINT16=3, /* 16-bit unsigned integer | */ - MAV_PARAM_TYPE_INT16=4, /* 16-bit signed integer | */ - MAV_PARAM_TYPE_UINT32=5, /* 32-bit unsigned integer | */ - MAV_PARAM_TYPE_INT32=6, /* 32-bit signed integer | */ - MAV_PARAM_TYPE_UINT64=7, /* 64-bit unsigned integer | */ - MAV_PARAM_TYPE_INT64=8, /* 64-bit signed integer | */ - MAV_PARAM_TYPE_REAL32=9, /* 32-bit floating-point | */ - MAV_PARAM_TYPE_REAL64=10, /* 64-bit floating-point | */ - MAV_PARAM_TYPE_ENUM_END=11, /* | */ -}; -#endif - -/** @brief result from a mavlink command */ -#ifndef HAVE_ENUM_MAV_RESULT -#define HAVE_ENUM_MAV_RESULT -enum MAV_RESULT -{ - MAV_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ - MAV_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */ - MAV_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */ - MAV_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */ - MAV_RESULT_FAILED=4, /* Command executed, but failed | */ - MAV_RESULT_ENUM_END=5, /* | */ -}; -#endif - -/** @brief result in a mavlink mission ack */ -#ifndef HAVE_ENUM_MAV_MISSION_RESULT -#define HAVE_ENUM_MAV_MISSION_RESULT -enum MAV_MISSION_RESULT -{ - MAV_MISSION_ACCEPTED=0, /* mission accepted OK | */ - MAV_MISSION_ERROR=1, /* generic error / not accepting mission commands at all right now | */ - MAV_MISSION_UNSUPPORTED_FRAME=2, /* coordinate frame is not supported | */ - MAV_MISSION_UNSUPPORTED=3, /* command is not supported | */ - MAV_MISSION_NO_SPACE=4, /* mission item exceeds storage space | */ - MAV_MISSION_INVALID=5, /* one of the parameters has an invalid value | */ - MAV_MISSION_INVALID_PARAM1=6, /* param1 has an invalid value | */ - MAV_MISSION_INVALID_PARAM2=7, /* param2 has an invalid value | */ - MAV_MISSION_INVALID_PARAM3=8, /* param3 has an invalid value | */ - MAV_MISSION_INVALID_PARAM4=9, /* param4 has an invalid value | */ - MAV_MISSION_INVALID_PARAM5_X=10, /* x/param5 has an invalid value | */ - MAV_MISSION_INVALID_PARAM6_Y=11, /* y/param6 has an invalid value | */ - MAV_MISSION_INVALID_PARAM7=12, /* param7 has an invalid value | */ - MAV_MISSION_INVALID_SEQUENCE=13, /* received waypoint out of sequence | */ - MAV_MISSION_DENIED=14, /* not accepting any mission commands from this communication partner | */ - MAV_MISSION_RESULT_ENUM_END=15, /* | */ -}; -#endif - -/** @brief Indicates the severity level, generally used for status messages to indicate their relative urgency. Based on RFC-5424 using expanded definitions at: http://www.kiwisyslog.com/kb/info:-syslog-message-levels/. */ -#ifndef HAVE_ENUM_MAV_SEVERITY -#define HAVE_ENUM_MAV_SEVERITY -enum MAV_SEVERITY -{ - MAV_SEVERITY_EMERGENCY=0, /* System is unusable. This is a "panic" condition. | */ - MAV_SEVERITY_ALERT=1, /* Action should be taken immediately. Indicates error in non-critical systems. | */ - MAV_SEVERITY_CRITICAL=2, /* Action must be taken immediately. Indicates failure in a primary system. | */ - MAV_SEVERITY_ERROR=3, /* Indicates an error in secondary/redundant systems. | */ - MAV_SEVERITY_WARNING=4, /* Indicates about a possible future error if this is not resolved within a given timeframe. Example would be a low battery warning. | */ - MAV_SEVERITY_NOTICE=5, /* An unusual event has occured, though not an error condition. This should be investigated for the root cause. | */ - MAV_SEVERITY_INFO=6, /* Normal operational messages. Useful for logging. No action is required for these messages. | */ - MAV_SEVERITY_DEBUG=7, /* Useful non-operational messages that can assist in debugging. These should not occur during normal operation. | */ - MAV_SEVERITY_ENUM_END=8, /* | */ -}; -#endif - -/** @brief Power supply status flags (bitmask) */ -#ifndef HAVE_ENUM_MAV_POWER_STATUS -#define HAVE_ENUM_MAV_POWER_STATUS -enum MAV_POWER_STATUS -{ - MAV_POWER_STATUS_BRICK_VALID=1, /* main brick power supply valid | */ - MAV_POWER_STATUS_SERVO_VALID=2, /* main servo power supply valid for FMU | */ - MAV_POWER_STATUS_USB_CONNECTED=4, /* USB power is connected | */ - MAV_POWER_STATUS_PERIPH_OVERCURRENT=8, /* peripheral supply is in over-current state | */ - MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT=16, /* hi-power peripheral supply is in over-current state | */ - MAV_POWER_STATUS_CHANGED=32, /* Power status has changed since boot | */ - MAV_POWER_STATUS_ENUM_END=33, /* | */ -}; -#endif - -/** @brief SERIAL_CONTROL device types */ -#ifndef HAVE_ENUM_SERIAL_CONTROL_DEV -#define HAVE_ENUM_SERIAL_CONTROL_DEV -enum SERIAL_CONTROL_DEV -{ - SERIAL_CONTROL_DEV_TELEM1=0, /* First telemetry port | */ - SERIAL_CONTROL_DEV_TELEM2=1, /* Second telemetry port | */ - SERIAL_CONTROL_DEV_GPS1=2, /* First GPS port | */ - SERIAL_CONTROL_DEV_GPS2=3, /* Second GPS port | */ - SERIAL_CONTROL_DEV_ENUM_END=4, /* | */ -}; -#endif - -/** @brief SERIAL_CONTROL flags (bitmask) */ -#ifndef HAVE_ENUM_SERIAL_CONTROL_FLAG -#define HAVE_ENUM_SERIAL_CONTROL_FLAG -enum SERIAL_CONTROL_FLAG -{ - SERIAL_CONTROL_FLAG_REPLY=1, /* Set if this is a reply | */ - SERIAL_CONTROL_FLAG_RESPOND=2, /* Set if the sender wants the receiver to send a response as another SERIAL_CONTROL message | */ - SERIAL_CONTROL_FLAG_EXCLUSIVE=4, /* Set if access to the serial port should be removed from whatever driver is currently using it, giving exclusive access to the SERIAL_CONTROL protocol. The port can be handed back by sending a request without this flag set | */ - SERIAL_CONTROL_FLAG_BLOCKING=8, /* Block on writes to the serial port | */ - SERIAL_CONTROL_FLAG_MULTI=16, /* Send multiple replies until port is drained | */ - SERIAL_CONTROL_FLAG_ENUM_END=17, /* | */ -}; -#endif - - - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_heartbeat.h" -#include "./mavlink_msg_sys_status.h" -#include "./mavlink_msg_system_time.h" -#include "./mavlink_msg_ping.h" -#include "./mavlink_msg_change_operator_control.h" -#include "./mavlink_msg_change_operator_control_ack.h" -#include "./mavlink_msg_auth_key.h" -#include "./mavlink_msg_set_mode.h" -#include "./mavlink_msg_param_request_read.h" -#include "./mavlink_msg_param_request_list.h" -#include "./mavlink_msg_param_value.h" -#include "./mavlink_msg_param_set.h" -#include "./mavlink_msg_gps_raw_int.h" -#include "./mavlink_msg_gps_status.h" -#include "./mavlink_msg_scaled_imu.h" -#include "./mavlink_msg_raw_imu.h" -#include "./mavlink_msg_raw_pressure.h" -#include "./mavlink_msg_scaled_pressure.h" -#include "./mavlink_msg_attitude.h" -#include "./mavlink_msg_attitude_quaternion.h" -#include "./mavlink_msg_local_position_ned.h" -#include "./mavlink_msg_global_position_int.h" -#include "./mavlink_msg_rc_channels_scaled.h" -#include "./mavlink_msg_rc_channels_raw.h" -#include "./mavlink_msg_servo_output_raw.h" -#include "./mavlink_msg_mission_request_partial_list.h" -#include "./mavlink_msg_mission_write_partial_list.h" -#include "./mavlink_msg_mission_item.h" -#include "./mavlink_msg_mission_request.h" -#include "./mavlink_msg_mission_set_current.h" -#include "./mavlink_msg_mission_current.h" -#include "./mavlink_msg_mission_request_list.h" -#include "./mavlink_msg_mission_count.h" -#include "./mavlink_msg_mission_clear_all.h" -#include "./mavlink_msg_mission_item_reached.h" -#include "./mavlink_msg_mission_ack.h" -#include "./mavlink_msg_set_gps_global_origin.h" -#include "./mavlink_msg_gps_global_origin.h" -#include "./mavlink_msg_set_local_position_setpoint.h" -#include "./mavlink_msg_local_position_setpoint.h" -#include "./mavlink_msg_global_position_setpoint_int.h" -#include "./mavlink_msg_set_global_position_setpoint_int.h" -#include "./mavlink_msg_safety_set_allowed_area.h" -#include "./mavlink_msg_safety_allowed_area.h" -#include "./mavlink_msg_set_roll_pitch_yaw_thrust.h" -#include "./mavlink_msg_set_roll_pitch_yaw_speed_thrust.h" -#include "./mavlink_msg_roll_pitch_yaw_thrust_setpoint.h" -#include "./mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h" -#include "./mavlink_msg_set_quad_motors_setpoint.h" -#include "./mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h" -#include "./mavlink_msg_nav_controller_output.h" -#include "./mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h" -#include "./mavlink_msg_state_correction.h" -#include "./mavlink_msg_rc_channels.h" -#include "./mavlink_msg_request_data_stream.h" -#include "./mavlink_msg_data_stream.h" -#include "./mavlink_msg_manual_control.h" -#include "./mavlink_msg_rc_channels_override.h" -#include "./mavlink_msg_vfr_hud.h" -#include "./mavlink_msg_command_long.h" -#include "./mavlink_msg_command_ack.h" -#include "./mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h" -#include "./mavlink_msg_manual_setpoint.h" -#include "./mavlink_msg_local_position_ned_system_global_offset.h" -#include "./mavlink_msg_hil_state.h" -#include "./mavlink_msg_hil_controls.h" -#include "./mavlink_msg_hil_rc_inputs_raw.h" -#include "./mavlink_msg_optical_flow.h" -#include "./mavlink_msg_global_vision_position_estimate.h" -#include "./mavlink_msg_vision_position_estimate.h" -#include "./mavlink_msg_vision_speed_estimate.h" -#include "./mavlink_msg_vicon_position_estimate.h" -#include "./mavlink_msg_highres_imu.h" -#include "./mavlink_msg_omnidirectional_flow.h" -#include "./mavlink_msg_hil_sensor.h" -#include "./mavlink_msg_sim_state.h" -#include "./mavlink_msg_radio_status.h" -#include "./mavlink_msg_file_transfer_start.h" -#include "./mavlink_msg_file_transfer_dir_list.h" -#include "./mavlink_msg_file_transfer_res.h" -#include "./mavlink_msg_hil_gps.h" -#include "./mavlink_msg_hil_optical_flow.h" -#include "./mavlink_msg_hil_state_quaternion.h" -#include "./mavlink_msg_scaled_imu2.h" -#include "./mavlink_msg_log_request_list.h" -#include "./mavlink_msg_log_entry.h" -#include "./mavlink_msg_log_request_data.h" -#include "./mavlink_msg_log_data.h" -#include "./mavlink_msg_log_erase.h" -#include "./mavlink_msg_log_request_end.h" -#include "./mavlink_msg_gps_inject_data.h" -#include "./mavlink_msg_gps2_raw.h" -#include "./mavlink_msg_power_status.h" -#include "./mavlink_msg_serial_control.h" -#include "./mavlink_msg_data_transmission_handshake.h" -#include "./mavlink_msg_encapsulated_data.h" -#include "./mavlink_msg_distance_sensor.h" -#include "./mavlink_msg_battery_status.h" -#include "./mavlink_msg_setpoint_8dof.h" -#include "./mavlink_msg_setpoint_6dof.h" -#include "./mavlink_msg_memory_vect.h" -#include "./mavlink_msg_debug_vect.h" -#include "./mavlink_msg_named_value_float.h" -#include "./mavlink_msg_named_value_int.h" -#include "./mavlink_msg_statustext.h" -#include "./mavlink_msg_debug.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // COMMON_H diff --git a/mavlink/include/mavlink/v1.0/common/mavlink.h b/mavlink/include/mavlink/v1.0/common/mavlink.h deleted file mode 100644 index 17b732970..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink.h +++ /dev/null @@ -1,27 +0,0 @@ -/** @file - * @brief MAVLink comm protocol built from common.xml - * @see http://pixhawk.ethz.ch/software/mavlink - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#ifndef MAVLINK_STX -#define MAVLINK_STX 254 -#endif - -#ifndef MAVLINK_ENDIAN -#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN -#endif - -#ifndef MAVLINK_ALIGNED_FIELDS -#define MAVLINK_ALIGNED_FIELDS 1 -#endif - -#ifndef MAVLINK_CRC_EXTRA -#define MAVLINK_CRC_EXTRA 1 -#endif - -#include "version.h" -#include "common.h" - -#endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h deleted file mode 100644 index ff2f104d4..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE ATTITUDE PACKING - -#define MAVLINK_MSG_ID_ATTITUDE 30 - -typedef struct __mavlink_attitude_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - float roll; ///< Roll angle (rad, -pi..+pi) - float pitch; ///< Pitch angle (rad, -pi..+pi) - float yaw; ///< Yaw angle (rad, -pi..+pi) - float rollspeed; ///< Roll angular speed (rad/s) - float pitchspeed; ///< Pitch angular speed (rad/s) - float yawspeed; ///< Yaw angular speed (rad/s) -} mavlink_attitude_t; - -#define MAVLINK_MSG_ID_ATTITUDE_LEN 28 -#define MAVLINK_MSG_ID_30_LEN 28 - -#define MAVLINK_MSG_ID_ATTITUDE_CRC 39 -#define MAVLINK_MSG_ID_30_CRC 39 - - - -#define MAVLINK_MESSAGE_INFO_ATTITUDE { \ - "ATTITUDE", \ - 7, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_t, time_boot_ms) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_t, yaw) }, \ - { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_attitude_t, rollspeed) }, \ - { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_t, pitchspeed) }, \ - { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_t, yawspeed) }, \ - } \ -} - - -/** - * @brief Pack a attitude message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param roll Roll angle (rad, -pi..+pi) - * @param pitch Pitch angle (rad, -pi..+pi) - * @param yaw Yaw angle (rad, -pi..+pi) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, rollspeed); - _mav_put_float(buf, 20, pitchspeed); - _mav_put_float(buf, 24, yawspeed); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_LEN); -#else - mavlink_attitude_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ATTITUDE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_LEN); -#endif -} - -/** - * @brief Pack a attitude message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param roll Roll angle (rad, -pi..+pi) - * @param pitch Pitch angle (rad, -pi..+pi) - * @param yaw Yaw angle (rad, -pi..+pi) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,float roll,float pitch,float yaw,float rollspeed,float pitchspeed,float yawspeed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, rollspeed); - _mav_put_float(buf, 20, pitchspeed); - _mav_put_float(buf, 24, yawspeed); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_LEN); -#else - mavlink_attitude_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ATTITUDE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_LEN); -#endif -} - -/** - * @brief Encode a attitude struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param attitude C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_t* attitude) -{ - return mavlink_msg_attitude_pack(system_id, component_id, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); -} - -/** - * @brief Encode a attitude struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param attitude C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_t* attitude) -{ - return mavlink_msg_attitude_pack_chan(system_id, component_id, chan, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); -} - -/** - * @brief Send a attitude message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param roll Roll angle (rad, -pi..+pi) - * @param pitch Pitch angle (rad, -pi..+pi) - * @param yaw Yaw angle (rad, -pi..+pi) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, rollspeed); - _mav_put_float(buf, 20, pitchspeed); - _mav_put_float(buf, 24, yawspeed); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN); -#endif -#else - mavlink_attitude_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, rollspeed); - _mav_put_float(buf, 20, pitchspeed); - _mav_put_float(buf, 24, yawspeed); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN); -#endif -#else - mavlink_attitude_t *packet = (mavlink_attitude_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - packet->rollspeed = rollspeed; - packet->pitchspeed = pitchspeed; - packet->yawspeed = yawspeed; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE ATTITUDE UNPACKING - - -/** - * @brief Get field time_boot_ms from attitude message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_attitude_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field roll from attitude message - * - * @return Roll angle (rad, -pi..+pi) - */ -static inline float mavlink_msg_attitude_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field pitch from attitude message - * - * @return Pitch angle (rad, -pi..+pi) - */ -static inline float mavlink_msg_attitude_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from attitude message - * - * @return Yaw angle (rad, -pi..+pi) - */ -static inline float mavlink_msg_attitude_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field rollspeed from attitude message - * - * @return Roll angular speed (rad/s) - */ -static inline float mavlink_msg_attitude_get_rollspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field pitchspeed from attitude message - * - * @return Pitch angular speed (rad/s) - */ -static inline float mavlink_msg_attitude_get_pitchspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field yawspeed from attitude message - * - * @return Yaw angular speed (rad/s) - */ -static inline float mavlink_msg_attitude_get_yawspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Decode a attitude message into a struct - * - * @param msg The message to decode - * @param attitude C-struct to decode the message contents into - */ -static inline void mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude) -{ -#if MAVLINK_NEED_BYTE_SWAP - attitude->time_boot_ms = mavlink_msg_attitude_get_time_boot_ms(msg); - attitude->roll = mavlink_msg_attitude_get_roll(msg); - attitude->pitch = mavlink_msg_attitude_get_pitch(msg); - attitude->yaw = mavlink_msg_attitude_get_yaw(msg); - attitude->rollspeed = mavlink_msg_attitude_get_rollspeed(msg); - attitude->pitchspeed = mavlink_msg_attitude_get_pitchspeed(msg); - attitude->yawspeed = mavlink_msg_attitude_get_yawspeed(msg); -#else - memcpy(attitude, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h deleted file mode 100644 index 3b97b40d0..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h +++ /dev/null @@ -1,377 +0,0 @@ -// MESSAGE ATTITUDE_QUATERNION PACKING - -#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION 31 - -typedef struct __mavlink_attitude_quaternion_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - float q1; ///< Quaternion component 1 - float q2; ///< Quaternion component 2 - float q3; ///< Quaternion component 3 - float q4; ///< Quaternion component 4 - float rollspeed; ///< Roll angular speed (rad/s) - float pitchspeed; ///< Pitch angular speed (rad/s) - float yawspeed; ///< Yaw angular speed (rad/s) -} mavlink_attitude_quaternion_t; - -#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN 32 -#define MAVLINK_MSG_ID_31_LEN 32 - -#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC 246 -#define MAVLINK_MSG_ID_31_CRC 246 - - - -#define MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION { \ - "ATTITUDE_QUATERNION", \ - 8, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_quaternion_t, time_boot_ms) }, \ - { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_quaternion_t, q1) }, \ - { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_quaternion_t, q2) }, \ - { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_quaternion_t, q3) }, \ - { "q4", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_attitude_quaternion_t, q4) }, \ - { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_quaternion_t, rollspeed) }, \ - { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_quaternion_t, pitchspeed) }, \ - { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_quaternion_t, yawspeed) }, \ - } \ -} - - -/** - * @brief Pack a attitude_quaternion message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param q1 Quaternion component 1 - * @param q2 Quaternion component 2 - * @param q3 Quaternion component 3 - * @param q4 Quaternion component 4 - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, q1); - _mav_put_float(buf, 8, q2); - _mav_put_float(buf, 12, q3); - _mav_put_float(buf, 16, q4); - _mav_put_float(buf, 20, rollspeed); - _mav_put_float(buf, 24, pitchspeed); - _mav_put_float(buf, 28, yawspeed); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#else - mavlink_attitude_quaternion_t packet; - packet.time_boot_ms = time_boot_ms; - packet.q1 = q1; - packet.q2 = q2; - packet.q3 = q3; - packet.q4 = q4; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif -} - -/** - * @brief Pack a attitude_quaternion message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param q1 Quaternion component 1 - * @param q2 Quaternion component 2 - * @param q3 Quaternion component 3 - * @param q4 Quaternion component 4 - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,float q1,float q2,float q3,float q4,float rollspeed,float pitchspeed,float yawspeed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, q1); - _mav_put_float(buf, 8, q2); - _mav_put_float(buf, 12, q3); - _mav_put_float(buf, 16, q4); - _mav_put_float(buf, 20, rollspeed); - _mav_put_float(buf, 24, pitchspeed); - _mav_put_float(buf, 28, yawspeed); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#else - mavlink_attitude_quaternion_t packet; - packet.time_boot_ms = time_boot_ms; - packet.q1 = q1; - packet.q2 = q2; - packet.q3 = q3; - packet.q4 = q4; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif -} - -/** - * @brief Encode a attitude_quaternion struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param attitude_quaternion C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) -{ - return mavlink_msg_attitude_quaternion_pack(system_id, component_id, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); -} - -/** - * @brief Encode a attitude_quaternion struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param attitude_quaternion C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) -{ - return mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, chan, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); -} - -/** - * @brief Send a attitude_quaternion message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param q1 Quaternion component 1 - * @param q2 Quaternion component 2 - * @param q3 Quaternion component 3 - * @param q4 Quaternion component 4 - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, q1); - _mav_put_float(buf, 8, q2); - _mav_put_float(buf, 12, q3); - _mav_put_float(buf, 16, q4); - _mav_put_float(buf, 20, rollspeed); - _mav_put_float(buf, 24, pitchspeed); - _mav_put_float(buf, 28, yawspeed); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif -#else - mavlink_attitude_quaternion_t packet; - packet.time_boot_ms = time_boot_ms; - packet.q1 = q1; - packet.q2 = q2; - packet.q3 = q3; - packet.q4 = q4; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, q1); - _mav_put_float(buf, 8, q2); - _mav_put_float(buf, 12, q3); - _mav_put_float(buf, 16, q4); - _mav_put_float(buf, 20, rollspeed); - _mav_put_float(buf, 24, pitchspeed); - _mav_put_float(buf, 28, yawspeed); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif -#else - mavlink_attitude_quaternion_t *packet = (mavlink_attitude_quaternion_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->q1 = q1; - packet->q2 = q2; - packet->q3 = q3; - packet->q4 = q4; - packet->rollspeed = rollspeed; - packet->pitchspeed = pitchspeed; - packet->yawspeed = yawspeed; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE ATTITUDE_QUATERNION UNPACKING - - -/** - * @brief Get field time_boot_ms from attitude_quaternion message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_attitude_quaternion_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field q1 from attitude_quaternion message - * - * @return Quaternion component 1 - */ -static inline float mavlink_msg_attitude_quaternion_get_q1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field q2 from attitude_quaternion message - * - * @return Quaternion component 2 - */ -static inline float mavlink_msg_attitude_quaternion_get_q2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field q3 from attitude_quaternion message - * - * @return Quaternion component 3 - */ -static inline float mavlink_msg_attitude_quaternion_get_q3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field q4 from attitude_quaternion message - * - * @return Quaternion component 4 - */ -static inline float mavlink_msg_attitude_quaternion_get_q4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field rollspeed from attitude_quaternion message - * - * @return Roll angular speed (rad/s) - */ -static inline float mavlink_msg_attitude_quaternion_get_rollspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field pitchspeed from attitude_quaternion message - * - * @return Pitch angular speed (rad/s) - */ -static inline float mavlink_msg_attitude_quaternion_get_pitchspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field yawspeed from attitude_quaternion message - * - * @return Yaw angular speed (rad/s) - */ -static inline float mavlink_msg_attitude_quaternion_get_yawspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a attitude_quaternion message into a struct - * - * @param msg The message to decode - * @param attitude_quaternion C-struct to decode the message contents into - */ -static inline void mavlink_msg_attitude_quaternion_decode(const mavlink_message_t* msg, mavlink_attitude_quaternion_t* attitude_quaternion) -{ -#if MAVLINK_NEED_BYTE_SWAP - attitude_quaternion->time_boot_ms = mavlink_msg_attitude_quaternion_get_time_boot_ms(msg); - attitude_quaternion->q1 = mavlink_msg_attitude_quaternion_get_q1(msg); - attitude_quaternion->q2 = mavlink_msg_attitude_quaternion_get_q2(msg); - attitude_quaternion->q3 = mavlink_msg_attitude_quaternion_get_q3(msg); - attitude_quaternion->q4 = mavlink_msg_attitude_quaternion_get_q4(msg); - attitude_quaternion->rollspeed = mavlink_msg_attitude_quaternion_get_rollspeed(msg); - attitude_quaternion->pitchspeed = mavlink_msg_attitude_quaternion_get_pitchspeed(msg); - attitude_quaternion->yawspeed = mavlink_msg_attitude_quaternion_get_yawspeed(msg); -#else - memcpy(attitude_quaternion, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h deleted file mode 100644 index f31b6bbf4..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h +++ /dev/null @@ -1,209 +0,0 @@ -// MESSAGE AUTH_KEY PACKING - -#define MAVLINK_MSG_ID_AUTH_KEY 7 - -typedef struct __mavlink_auth_key_t -{ - char key[32]; ///< key -} mavlink_auth_key_t; - -#define MAVLINK_MSG_ID_AUTH_KEY_LEN 32 -#define MAVLINK_MSG_ID_7_LEN 32 - -#define MAVLINK_MSG_ID_AUTH_KEY_CRC 119 -#define MAVLINK_MSG_ID_7_CRC 119 - -#define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32 - -#define MAVLINK_MESSAGE_INFO_AUTH_KEY { \ - "AUTH_KEY", \ - 1, \ - { { "key", NULL, MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_auth_key_t, key) }, \ - } \ -} - - -/** - * @brief Pack a auth_key message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param key key - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const char *key) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN]; - - _mav_put_char_array(buf, 0, key, 32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); -#else - mavlink_auth_key_t packet; - - mav_array_memcpy(packet.key, key, sizeof(char)*32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN); -#endif -} - -/** - * @brief Pack a auth_key message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param key key - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const char *key) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN]; - - _mav_put_char_array(buf, 0, key, 32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); -#else - mavlink_auth_key_t packet; - - mav_array_memcpy(packet.key, key, sizeof(char)*32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN); -#endif -} - -/** - * @brief Encode a auth_key struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param auth_key C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key) -{ - return mavlink_msg_auth_key_pack(system_id, component_id, msg, auth_key->key); -} - -/** - * @brief Encode a auth_key struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param auth_key C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_auth_key_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key) -{ - return mavlink_msg_auth_key_pack_chan(system_id, component_id, chan, msg, auth_key->key); -} - -/** - * @brief Send a auth_key message - * @param chan MAVLink channel to send the message - * - * @param key key - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char *key) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN]; - - _mav_put_char_array(buf, 0, key, 32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); -#endif -#else - mavlink_auth_key_t packet; - - mav_array_memcpy(packet.key, key, sizeof(char)*32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_AUTH_KEY_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_auth_key_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *key) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - - _mav_put_char_array(buf, 0, key, 32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); -#endif -#else - mavlink_auth_key_t *packet = (mavlink_auth_key_t *)msgbuf; - - mav_array_memcpy(packet->key, key, sizeof(char)*32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE AUTH_KEY UNPACKING - - -/** - * @brief Get field key from auth_key message - * - * @return key - */ -static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char *key) -{ - return _MAV_RETURN_char_array(msg, key, 32, 0); -} - -/** - * @brief Decode a auth_key message into a struct - * - * @param msg The message to decode - * @param auth_key C-struct to decode the message contents into - */ -static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mavlink_auth_key_t* auth_key) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_auth_key_get_key(msg, auth_key->key); -#else - memcpy(auth_key, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AUTH_KEY_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h deleted file mode 100644 index faaabf9f7..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h +++ /dev/null @@ -1,449 +0,0 @@ -// MESSAGE BATTERY_STATUS PACKING - -#define MAVLINK_MSG_ID_BATTERY_STATUS 147 - -typedef struct __mavlink_battery_status_t -{ - int32_t current_consumed; ///< Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate - int32_t energy_consumed; ///< Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate - uint16_t voltage_cell_1; ///< Battery voltage of cell 1, in millivolts (1 = 1 millivolt) - uint16_t voltage_cell_2; ///< Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell - uint16_t voltage_cell_3; ///< Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell - uint16_t voltage_cell_4; ///< Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell - uint16_t voltage_cell_5; ///< Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell - uint16_t voltage_cell_6; ///< Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell - int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - uint8_t accu_id; ///< Accupack ID - int8_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery -} mavlink_battery_status_t; - -#define MAVLINK_MSG_ID_BATTERY_STATUS_LEN 24 -#define MAVLINK_MSG_ID_147_LEN 24 - -#define MAVLINK_MSG_ID_BATTERY_STATUS_CRC 177 -#define MAVLINK_MSG_ID_147_CRC 177 - - - -#define MAVLINK_MESSAGE_INFO_BATTERY_STATUS { \ - "BATTERY_STATUS", \ - 11, \ - { { "current_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_battery_status_t, current_consumed) }, \ - { "energy_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_battery_status_t, energy_consumed) }, \ - { "voltage_cell_1", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_battery_status_t, voltage_cell_1) }, \ - { "voltage_cell_2", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_battery_status_t, voltage_cell_2) }, \ - { "voltage_cell_3", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_battery_status_t, voltage_cell_3) }, \ - { "voltage_cell_4", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_battery_status_t, voltage_cell_4) }, \ - { "voltage_cell_5", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_battery_status_t, voltage_cell_5) }, \ - { "voltage_cell_6", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_battery_status_t, voltage_cell_6) }, \ - { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_battery_status_t, current_battery) }, \ - { "accu_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_battery_status_t, accu_id) }, \ - { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 23, offsetof(mavlink_battery_status_t, battery_remaining) }, \ - } \ -} - - -/** - * @brief Pack a battery_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param accu_id Accupack ID - * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt) - * @param voltage_cell_2 Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_3 Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_4 Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell - * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - * @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate - * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate - * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; - _mav_put_int32_t(buf, 0, current_consumed); - _mav_put_int32_t(buf, 4, energy_consumed); - _mav_put_uint16_t(buf, 8, voltage_cell_1); - _mav_put_uint16_t(buf, 10, voltage_cell_2); - _mav_put_uint16_t(buf, 12, voltage_cell_3); - _mav_put_uint16_t(buf, 14, voltage_cell_4); - _mav_put_uint16_t(buf, 16, voltage_cell_5); - _mav_put_uint16_t(buf, 18, voltage_cell_6); - _mav_put_int16_t(buf, 20, current_battery); - _mav_put_uint8_t(buf, 22, accu_id); - _mav_put_int8_t(buf, 23, battery_remaining); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); -#else - mavlink_battery_status_t packet; - packet.current_consumed = current_consumed; - packet.energy_consumed = energy_consumed; - packet.voltage_cell_1 = voltage_cell_1; - packet.voltage_cell_2 = voltage_cell_2; - packet.voltage_cell_3 = voltage_cell_3; - packet.voltage_cell_4 = voltage_cell_4; - packet.voltage_cell_5 = voltage_cell_5; - packet.voltage_cell_6 = voltage_cell_6; - packet.current_battery = current_battery; - packet.accu_id = accu_id; - packet.battery_remaining = battery_remaining; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_BATTERY_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); -#endif -} - -/** - * @brief Pack a battery_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param accu_id Accupack ID - * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt) - * @param voltage_cell_2 Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_3 Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_4 Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell - * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - * @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate - * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate - * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t accu_id,uint16_t voltage_cell_1,uint16_t voltage_cell_2,uint16_t voltage_cell_3,uint16_t voltage_cell_4,uint16_t voltage_cell_5,uint16_t voltage_cell_6,int16_t current_battery,int32_t current_consumed,int32_t energy_consumed,int8_t battery_remaining) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; - _mav_put_int32_t(buf, 0, current_consumed); - _mav_put_int32_t(buf, 4, energy_consumed); - _mav_put_uint16_t(buf, 8, voltage_cell_1); - _mav_put_uint16_t(buf, 10, voltage_cell_2); - _mav_put_uint16_t(buf, 12, voltage_cell_3); - _mav_put_uint16_t(buf, 14, voltage_cell_4); - _mav_put_uint16_t(buf, 16, voltage_cell_5); - _mav_put_uint16_t(buf, 18, voltage_cell_6); - _mav_put_int16_t(buf, 20, current_battery); - _mav_put_uint8_t(buf, 22, accu_id); - _mav_put_int8_t(buf, 23, battery_remaining); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); -#else - mavlink_battery_status_t packet; - packet.current_consumed = current_consumed; - packet.energy_consumed = energy_consumed; - packet.voltage_cell_1 = voltage_cell_1; - packet.voltage_cell_2 = voltage_cell_2; - packet.voltage_cell_3 = voltage_cell_3; - packet.voltage_cell_4 = voltage_cell_4; - packet.voltage_cell_5 = voltage_cell_5; - packet.voltage_cell_6 = voltage_cell_6; - packet.current_battery = current_battery; - packet.accu_id = accu_id; - packet.battery_remaining = battery_remaining; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_BATTERY_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); -#endif -} - -/** - * @brief Encode a battery_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param battery_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) -{ - return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); -} - -/** - * @brief Encode a battery_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param battery_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) -{ - return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); -} - -/** - * @brief Send a battery_status message - * @param chan MAVLink channel to send the message - * - * @param accu_id Accupack ID - * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt) - * @param voltage_cell_2 Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_3 Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_4 Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell - * @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell - * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - * @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate - * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate - * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; - _mav_put_int32_t(buf, 0, current_consumed); - _mav_put_int32_t(buf, 4, energy_consumed); - _mav_put_uint16_t(buf, 8, voltage_cell_1); - _mav_put_uint16_t(buf, 10, voltage_cell_2); - _mav_put_uint16_t(buf, 12, voltage_cell_3); - _mav_put_uint16_t(buf, 14, voltage_cell_4); - _mav_put_uint16_t(buf, 16, voltage_cell_5); - _mav_put_uint16_t(buf, 18, voltage_cell_6); - _mav_put_int16_t(buf, 20, current_battery); - _mav_put_uint8_t(buf, 22, accu_id); - _mav_put_int8_t(buf, 23, battery_remaining); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); -#endif -#else - mavlink_battery_status_t packet; - packet.current_consumed = current_consumed; - packet.energy_consumed = energy_consumed; - packet.voltage_cell_1 = voltage_cell_1; - packet.voltage_cell_2 = voltage_cell_2; - packet.voltage_cell_3 = voltage_cell_3; - packet.voltage_cell_4 = voltage_cell_4; - packet.voltage_cell_5 = voltage_cell_5; - packet.voltage_cell_6 = voltage_cell_6; - packet.current_battery = current_battery; - packet.accu_id = accu_id; - packet.battery_remaining = battery_remaining; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)&packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)&packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_BATTERY_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, current_consumed); - _mav_put_int32_t(buf, 4, energy_consumed); - _mav_put_uint16_t(buf, 8, voltage_cell_1); - _mav_put_uint16_t(buf, 10, voltage_cell_2); - _mav_put_uint16_t(buf, 12, voltage_cell_3); - _mav_put_uint16_t(buf, 14, voltage_cell_4); - _mav_put_uint16_t(buf, 16, voltage_cell_5); - _mav_put_uint16_t(buf, 18, voltage_cell_6); - _mav_put_int16_t(buf, 20, current_battery); - _mav_put_uint8_t(buf, 22, accu_id); - _mav_put_int8_t(buf, 23, battery_remaining); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); -#endif -#else - mavlink_battery_status_t *packet = (mavlink_battery_status_t *)msgbuf; - packet->current_consumed = current_consumed; - packet->energy_consumed = energy_consumed; - packet->voltage_cell_1 = voltage_cell_1; - packet->voltage_cell_2 = voltage_cell_2; - packet->voltage_cell_3 = voltage_cell_3; - packet->voltage_cell_4 = voltage_cell_4; - packet->voltage_cell_5 = voltage_cell_5; - packet->voltage_cell_6 = voltage_cell_6; - packet->current_battery = current_battery; - packet->accu_id = accu_id; - packet->battery_remaining = battery_remaining; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE BATTERY_STATUS UNPACKING - - -/** - * @brief Get field accu_id from battery_status message - * - * @return Accupack ID - */ -static inline uint8_t mavlink_msg_battery_status_get_accu_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 22); -} - -/** - * @brief Get field voltage_cell_1 from battery_status message - * - * @return Battery voltage of cell 1, in millivolts (1 = 1 millivolt) - */ -static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field voltage_cell_2 from battery_status message - * - * @return Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell - */ -static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Get field voltage_cell_3 from battery_status message - * - * @return Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell - */ -static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field voltage_cell_4 from battery_status message - * - * @return Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell - */ -static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 14); -} - -/** - * @brief Get field voltage_cell_5 from battery_status message - * - * @return Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell - */ -static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field voltage_cell_6 from battery_status message - * - * @return Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell - */ -static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 18); -} - -/** - * @brief Get field current_battery from battery_status message - * - * @return Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - */ -static inline int16_t mavlink_msg_battery_status_get_current_battery(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Get field current_consumed from battery_status message - * - * @return Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate - */ -static inline int32_t mavlink_msg_battery_status_get_current_consumed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field energy_consumed from battery_status message - * - * @return Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate - */ -static inline int32_t mavlink_msg_battery_status_get_energy_consumed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field battery_remaining from battery_status message - * - * @return Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery - */ -static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int8_t(msg, 23); -} - -/** - * @brief Decode a battery_status message into a struct - * - * @param msg The message to decode - * @param battery_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_battery_status_decode(const mavlink_message_t* msg, mavlink_battery_status_t* battery_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - battery_status->current_consumed = mavlink_msg_battery_status_get_current_consumed(msg); - battery_status->energy_consumed = mavlink_msg_battery_status_get_energy_consumed(msg); - battery_status->voltage_cell_1 = mavlink_msg_battery_status_get_voltage_cell_1(msg); - battery_status->voltage_cell_2 = mavlink_msg_battery_status_get_voltage_cell_2(msg); - battery_status->voltage_cell_3 = mavlink_msg_battery_status_get_voltage_cell_3(msg); - battery_status->voltage_cell_4 = mavlink_msg_battery_status_get_voltage_cell_4(msg); - battery_status->voltage_cell_5 = mavlink_msg_battery_status_get_voltage_cell_5(msg); - battery_status->voltage_cell_6 = mavlink_msg_battery_status_get_voltage_cell_6(msg); - battery_status->current_battery = mavlink_msg_battery_status_get_current_battery(msg); - battery_status->accu_id = mavlink_msg_battery_status_get_accu_id(msg); - battery_status->battery_remaining = mavlink_msg_battery_status_get_battery_remaining(msg); -#else - memcpy(battery_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_BATTERY_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h deleted file mode 100644 index 3f0987f10..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h +++ /dev/null @@ -1,273 +0,0 @@ -// MESSAGE CHANGE_OPERATOR_CONTROL PACKING - -#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL 5 - -typedef struct __mavlink_change_operator_control_t -{ - uint8_t target_system; ///< System the GCS requests control for - uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV - uint8_t version; ///< 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - char passkey[25]; ///< Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" -} mavlink_change_operator_control_t; - -#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN 28 -#define MAVLINK_MSG_ID_5_LEN 28 - -#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC 217 -#define MAVLINK_MSG_ID_5_CRC 217 - -#define MAVLINK_MSG_CHANGE_OPERATOR_CONTROL_FIELD_PASSKEY_LEN 25 - -#define MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL { \ - "CHANGE_OPERATOR_CONTROL", \ - 4, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_t, target_system) }, \ - { "control_request", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_t, control_request) }, \ - { "version", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_t, version) }, \ - { "passkey", NULL, MAVLINK_TYPE_CHAR, 25, 3, offsetof(mavlink_change_operator_control_t, passkey) }, \ - } \ -} - - -/** - * @brief Pack a change_operator_control message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System the GCS requests control for - * @param control_request 0: request control of this MAV, 1: Release control of this MAV - * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, control_request); - _mav_put_uint8_t(buf, 2, version); - _mav_put_char_array(buf, 3, passkey, 25); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); -#else - mavlink_change_operator_control_t packet; - packet.target_system = target_system; - packet.control_request = control_request; - packet.version = version; - mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); -#endif -} - -/** - * @brief Pack a change_operator_control message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System the GCS requests control for - * @param control_request 0: request control of this MAV, 1: Release control of this MAV - * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t control_request,uint8_t version,const char *passkey) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, control_request); - _mav_put_uint8_t(buf, 2, version); - _mav_put_char_array(buf, 3, passkey, 25); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); -#else - mavlink_change_operator_control_t packet; - packet.target_system = target_system; - packet.control_request = control_request; - packet.version = version; - mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); -#endif -} - -/** - * @brief Encode a change_operator_control struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param change_operator_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_change_operator_control_t* change_operator_control) -{ - return mavlink_msg_change_operator_control_pack(system_id, component_id, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); -} - -/** - * @brief Encode a change_operator_control struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param change_operator_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_change_operator_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_change_operator_control_t* change_operator_control) -{ - return mavlink_msg_change_operator_control_pack_chan(system_id, component_id, chan, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); -} - -/** - * @brief Send a change_operator_control message - * @param chan MAVLink channel to send the message - * - * @param target_system System the GCS requests control for - * @param control_request 0: request control of this MAV, 1: Release control of this MAV - * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, control_request); - _mav_put_uint8_t(buf, 2, version); - _mav_put_char_array(buf, 3, passkey, 25); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); -#endif -#else - mavlink_change_operator_control_t packet; - packet.target_system = target_system; - packet.control_request = control_request; - packet.version = version; - mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_change_operator_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, control_request); - _mav_put_uint8_t(buf, 2, version); - _mav_put_char_array(buf, 3, passkey, 25); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); -#endif -#else - mavlink_change_operator_control_t *packet = (mavlink_change_operator_control_t *)msgbuf; - packet->target_system = target_system; - packet->control_request = control_request; - packet->version = version; - mav_array_memcpy(packet->passkey, passkey, sizeof(char)*25); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE CHANGE_OPERATOR_CONTROL UNPACKING - - -/** - * @brief Get field target_system from change_operator_control message - * - * @return System the GCS requests control for - */ -static inline uint8_t mavlink_msg_change_operator_control_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field control_request from change_operator_control message - * - * @return 0: request control of this MAV, 1: Release control of this MAV - */ -static inline uint8_t mavlink_msg_change_operator_control_get_control_request(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field version from change_operator_control message - * - * @return 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - */ -static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field passkey from change_operator_control message - * - * @return Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - */ -static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mavlink_message_t* msg, char *passkey) -{ - return _MAV_RETURN_char_array(msg, passkey, 25, 3); -} - -/** - * @brief Decode a change_operator_control message into a struct - * - * @param msg The message to decode - * @param change_operator_control C-struct to decode the message contents into - */ -static inline void mavlink_msg_change_operator_control_decode(const mavlink_message_t* msg, mavlink_change_operator_control_t* change_operator_control) -{ -#if MAVLINK_NEED_BYTE_SWAP - change_operator_control->target_system = mavlink_msg_change_operator_control_get_target_system(msg); - change_operator_control->control_request = mavlink_msg_change_operator_control_get_control_request(msg); - change_operator_control->version = mavlink_msg_change_operator_control_get_version(msg); - mavlink_msg_change_operator_control_get_passkey(msg, change_operator_control->passkey); -#else - memcpy(change_operator_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h deleted file mode 100644 index 768e7ed5c..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE CHANGE_OPERATOR_CONTROL_ACK PACKING - -#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK 6 - -typedef struct __mavlink_change_operator_control_ack_t -{ - uint8_t gcs_system_id; ///< ID of the GCS this message - uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV - uint8_t ack; ///< 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control -} mavlink_change_operator_control_ack_t; - -#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN 3 -#define MAVLINK_MSG_ID_6_LEN 3 - -#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC 104 -#define MAVLINK_MSG_ID_6_CRC 104 - - - -#define MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK { \ - "CHANGE_OPERATOR_CONTROL_ACK", \ - 3, \ - { { "gcs_system_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_ack_t, gcs_system_id) }, \ - { "control_request", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_ack_t, control_request) }, \ - { "ack", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_ack_t, ack) }, \ - } \ -} - - -/** - * @brief Pack a change_operator_control_ack message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param gcs_system_id ID of the GCS this message - * @param control_request 0: request control of this MAV, 1: Release control of this MAV - * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN]; - _mav_put_uint8_t(buf, 0, gcs_system_id); - _mav_put_uint8_t(buf, 1, control_request); - _mav_put_uint8_t(buf, 2, ack); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); -#else - mavlink_change_operator_control_ack_t packet; - packet.gcs_system_id = gcs_system_id; - packet.control_request = control_request; - packet.ack = ack; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); -#endif -} - -/** - * @brief Pack a change_operator_control_ack message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param gcs_system_id ID of the GCS this message - * @param control_request 0: request control of this MAV, 1: Release control of this MAV - * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t gcs_system_id,uint8_t control_request,uint8_t ack) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN]; - _mav_put_uint8_t(buf, 0, gcs_system_id); - _mav_put_uint8_t(buf, 1, control_request); - _mav_put_uint8_t(buf, 2, ack); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); -#else - mavlink_change_operator_control_ack_t packet; - packet.gcs_system_id = gcs_system_id; - packet.control_request = control_request; - packet.ack = ack; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); -#endif -} - -/** - * @brief Encode a change_operator_control_ack struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param change_operator_control_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_change_operator_control_ack_t* change_operator_control_ack) -{ - return mavlink_msg_change_operator_control_ack_pack(system_id, component_id, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); -} - -/** - * @brief Encode a change_operator_control_ack struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param change_operator_control_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_change_operator_control_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_change_operator_control_ack_t* change_operator_control_ack) -{ - return mavlink_msg_change_operator_control_ack_pack_chan(system_id, component_id, chan, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); -} - -/** - * @brief Send a change_operator_control_ack message - * @param chan MAVLink channel to send the message - * - * @param gcs_system_id ID of the GCS this message - * @param control_request 0: request control of this MAV, 1: Release control of this MAV - * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN]; - _mav_put_uint8_t(buf, 0, gcs_system_id); - _mav_put_uint8_t(buf, 1, control_request); - _mav_put_uint8_t(buf, 2, ack); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); -#endif -#else - mavlink_change_operator_control_ack_t packet; - packet.gcs_system_id = gcs_system_id; - packet.control_request = control_request; - packet.ack = ack; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)&packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)&packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_change_operator_control_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, gcs_system_id); - _mav_put_uint8_t(buf, 1, control_request); - _mav_put_uint8_t(buf, 2, ack); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); -#endif -#else - mavlink_change_operator_control_ack_t *packet = (mavlink_change_operator_control_ack_t *)msgbuf; - packet->gcs_system_id = gcs_system_id; - packet->control_request = control_request; - packet->ack = ack; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE CHANGE_OPERATOR_CONTROL_ACK UNPACKING - - -/** - * @brief Get field gcs_system_id from change_operator_control_ack message - * - * @return ID of the GCS this message - */ -static inline uint8_t mavlink_msg_change_operator_control_ack_get_gcs_system_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field control_request from change_operator_control_ack message - * - * @return 0: request control of this MAV, 1: Release control of this MAV - */ -static inline uint8_t mavlink_msg_change_operator_control_ack_get_control_request(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field ack from change_operator_control_ack message - * - * @return 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - */ -static inline uint8_t mavlink_msg_change_operator_control_ack_get_ack(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a change_operator_control_ack message into a struct - * - * @param msg The message to decode - * @param change_operator_control_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_change_operator_control_ack_decode(const mavlink_message_t* msg, mavlink_change_operator_control_ack_t* change_operator_control_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - change_operator_control_ack->gcs_system_id = mavlink_msg_change_operator_control_ack_get_gcs_system_id(msg); - change_operator_control_ack->control_request = mavlink_msg_change_operator_control_ack_get_control_request(msg); - change_operator_control_ack->ack = mavlink_msg_change_operator_control_ack_get_ack(msg); -#else - memcpy(change_operator_control_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h deleted file mode 100644 index d3d163041..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE COMMAND_ACK PACKING - -#define MAVLINK_MSG_ID_COMMAND_ACK 77 - -typedef struct __mavlink_command_ack_t -{ - uint16_t command; ///< Command ID, as defined by MAV_CMD enum. - uint8_t result; ///< See MAV_RESULT enum -} mavlink_command_ack_t; - -#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 3 -#define MAVLINK_MSG_ID_77_LEN 3 - -#define MAVLINK_MSG_ID_COMMAND_ACK_CRC 143 -#define MAVLINK_MSG_ID_77_CRC 143 - - - -#define MAVLINK_MESSAGE_INFO_COMMAND_ACK { \ - "COMMAND_ACK", \ - 2, \ - { { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_command_ack_t, command) }, \ - { "result", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_command_ack_t, result) }, \ - } \ -} - - -/** - * @brief Pack a command_ack message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param command Command ID, as defined by MAV_CMD enum. - * @param result See MAV_RESULT enum - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t command, uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; - _mav_put_uint16_t(buf, 0, command); - _mav_put_uint8_t(buf, 2, result); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); -#else - mavlink_command_ack_t packet; - packet.command = command; - packet.result = result; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_ACK_LEN); -#endif -} - -/** - * @brief Pack a command_ack message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param command Command ID, as defined by MAV_CMD enum. - * @param result See MAV_RESULT enum - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t command,uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; - _mav_put_uint16_t(buf, 0, command); - _mav_put_uint8_t(buf, 2, result); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); -#else - mavlink_command_ack_t packet; - packet.command = command; - packet.result = result; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_ACK_LEN); -#endif -} - -/** - * @brief Encode a command_ack struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param command_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) -{ - return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result); -} - -/** - * @brief Encode a command_ack struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param command_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) -{ - return mavlink_msg_command_ack_pack_chan(system_id, component_id, chan, msg, command_ack->command, command_ack->result); -} - -/** - * @brief Send a command_ack message - * @param chan MAVLink channel to send the message - * - * @param command Command ID, as defined by MAV_CMD enum. - * @param result See MAV_RESULT enum - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t command, uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; - _mav_put_uint16_t(buf, 0, command); - _mav_put_uint8_t(buf, 2, result); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); -#endif -#else - mavlink_command_ack_t packet; - packet.command = command; - packet.result = result; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_COMMAND_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t command, uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, command); - _mav_put_uint8_t(buf, 2, result); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); -#endif -#else - mavlink_command_ack_t *packet = (mavlink_command_ack_t *)msgbuf; - packet->command = command; - packet->result = result; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE COMMAND_ACK UNPACKING - - -/** - * @brief Get field command from command_ack message - * - * @return Command ID, as defined by MAV_CMD enum. - */ -static inline uint16_t mavlink_msg_command_ack_get_command(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field result from command_ack message - * - * @return See MAV_RESULT enum - */ -static inline uint8_t mavlink_msg_command_ack_get_result(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a command_ack message into a struct - * - * @param msg The message to decode - * @param command_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_command_ack_decode(const mavlink_message_t* msg, mavlink_command_ack_t* command_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - command_ack->command = mavlink_msg_command_ack_get_command(msg); - command_ack->result = mavlink_msg_command_ack_get_result(msg); -#else - memcpy(command_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMMAND_ACK_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h deleted file mode 100644 index 161896b97..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h +++ /dev/null @@ -1,449 +0,0 @@ -// MESSAGE COMMAND_LONG PACKING - -#define MAVLINK_MSG_ID_COMMAND_LONG 76 - -typedef struct __mavlink_command_long_t -{ - float param1; ///< Parameter 1, as defined by MAV_CMD enum. - float param2; ///< Parameter 2, as defined by MAV_CMD enum. - float param3; ///< Parameter 3, as defined by MAV_CMD enum. - float param4; ///< Parameter 4, as defined by MAV_CMD enum. - float param5; ///< Parameter 5, as defined by MAV_CMD enum. - float param6; ///< Parameter 6, as defined by MAV_CMD enum. - float param7; ///< Parameter 7, as defined by MAV_CMD enum. - uint16_t command; ///< Command ID, as defined by MAV_CMD enum. - uint8_t target_system; ///< System which should execute the command - uint8_t target_component; ///< Component which should execute the command, 0 for all components - uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) -} mavlink_command_long_t; - -#define MAVLINK_MSG_ID_COMMAND_LONG_LEN 33 -#define MAVLINK_MSG_ID_76_LEN 33 - -#define MAVLINK_MSG_ID_COMMAND_LONG_CRC 152 -#define MAVLINK_MSG_ID_76_CRC 152 - - - -#define MAVLINK_MESSAGE_INFO_COMMAND_LONG { \ - "COMMAND_LONG", \ - 11, \ - { { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_command_long_t, param1) }, \ - { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_long_t, param2) }, \ - { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_command_long_t, param3) }, \ - { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_command_long_t, param4) }, \ - { "param5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_command_long_t, param5) }, \ - { "param6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_command_long_t, param6) }, \ - { "param7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_command_long_t, param7) }, \ - { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_command_long_t, command) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_command_long_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_command_long_t, target_component) }, \ - { "confirmation", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_command_long_t, confirmation) }, \ - } \ -} - - -/** - * @brief Pack a command_long message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param command Command ID, as defined by MAV_CMD enum. - * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - * @param param1 Parameter 1, as defined by MAV_CMD enum. - * @param param2 Parameter 2, as defined by MAV_CMD enum. - * @param param3 Parameter 3, as defined by MAV_CMD enum. - * @param param4 Parameter 4, as defined by MAV_CMD enum. - * @param param5 Parameter 5, as defined by MAV_CMD enum. - * @param param6 Parameter 6, as defined by MAV_CMD enum. - * @param param7 Parameter 7, as defined by MAV_CMD enum. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_COMMAND_LONG_LEN]; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_float(buf, 16, param5); - _mav_put_float(buf, 20, param6); - _mav_put_float(buf, 24, param7); - _mav_put_uint16_t(buf, 28, command); - _mav_put_uint8_t(buf, 30, target_system); - _mav_put_uint8_t(buf, 31, target_component); - _mav_put_uint8_t(buf, 32, confirmation); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN); -#else - mavlink_command_long_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.param5 = param5; - packet.param6 = param6; - packet.param7 = param7; - packet.command = command; - packet.target_system = target_system; - packet.target_component = target_component; - packet.confirmation = confirmation; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LONG_LEN); -#endif -} - -/** - * @brief Pack a command_long message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param command Command ID, as defined by MAV_CMD enum. - * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - * @param param1 Parameter 1, as defined by MAV_CMD enum. - * @param param2 Parameter 2, as defined by MAV_CMD enum. - * @param param3 Parameter 3, as defined by MAV_CMD enum. - * @param param4 Parameter 4, as defined by MAV_CMD enum. - * @param param5 Parameter 5, as defined by MAV_CMD enum. - * @param param6 Parameter 6, as defined by MAV_CMD enum. - * @param param7 Parameter 7, as defined by MAV_CMD enum. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t command,uint8_t confirmation,float param1,float param2,float param3,float param4,float param5,float param6,float param7) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_COMMAND_LONG_LEN]; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_float(buf, 16, param5); - _mav_put_float(buf, 20, param6); - _mav_put_float(buf, 24, param7); - _mav_put_uint16_t(buf, 28, command); - _mav_put_uint8_t(buf, 30, target_system); - _mav_put_uint8_t(buf, 31, target_component); - _mav_put_uint8_t(buf, 32, confirmation); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN); -#else - mavlink_command_long_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.param5 = param5; - packet.param6 = param6; - packet.param7 = param7; - packet.command = command; - packet.target_system = target_system; - packet.target_component = target_component; - packet.confirmation = confirmation; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_LONG_LEN); -#endif -} - -/** - * @brief Encode a command_long struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param command_long C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_command_long_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_long_t* command_long) -{ - return mavlink_msg_command_long_pack(system_id, component_id, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7); -} - -/** - * @brief Encode a command_long struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param command_long C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_command_long_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_long_t* command_long) -{ - return mavlink_msg_command_long_pack_chan(system_id, component_id, chan, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7); -} - -/** - * @brief Send a command_long message - * @param chan MAVLink channel to send the message - * - * @param target_system System which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param command Command ID, as defined by MAV_CMD enum. - * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - * @param param1 Parameter 1, as defined by MAV_CMD enum. - * @param param2 Parameter 2, as defined by MAV_CMD enum. - * @param param3 Parameter 3, as defined by MAV_CMD enum. - * @param param4 Parameter 4, as defined by MAV_CMD enum. - * @param param5 Parameter 5, as defined by MAV_CMD enum. - * @param param6 Parameter 6, as defined by MAV_CMD enum. - * @param param7 Parameter 7, as defined by MAV_CMD enum. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_COMMAND_LONG_LEN]; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_float(buf, 16, param5); - _mav_put_float(buf, 20, param6); - _mav_put_float(buf, 24, param7); - _mav_put_uint16_t(buf, 28, command); - _mav_put_uint8_t(buf, 30, target_system); - _mav_put_uint8_t(buf, 31, target_component); - _mav_put_uint8_t(buf, 32, confirmation); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN); -#endif -#else - mavlink_command_long_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.param5 = param5; - packet.param6 = param6; - packet.param7 = param7; - packet.command = command; - packet.target_system = target_system; - packet.target_component = target_component; - packet.confirmation = confirmation; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_COMMAND_LONG_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_command_long_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_float(buf, 16, param5); - _mav_put_float(buf, 20, param6); - _mav_put_float(buf, 24, param7); - _mav_put_uint16_t(buf, 28, command); - _mav_put_uint8_t(buf, 30, target_system); - _mav_put_uint8_t(buf, 31, target_component); - _mav_put_uint8_t(buf, 32, confirmation); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN); -#endif -#else - mavlink_command_long_t *packet = (mavlink_command_long_t *)msgbuf; - packet->param1 = param1; - packet->param2 = param2; - packet->param3 = param3; - packet->param4 = param4; - packet->param5 = param5; - packet->param6 = param6; - packet->param7 = param7; - packet->command = command; - packet->target_system = target_system; - packet->target_component = target_component; - packet->confirmation = confirmation; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE COMMAND_LONG UNPACKING - - -/** - * @brief Get field target_system from command_long message - * - * @return System which should execute the command - */ -static inline uint8_t mavlink_msg_command_long_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 30); -} - -/** - * @brief Get field target_component from command_long message - * - * @return Component which should execute the command, 0 for all components - */ -static inline uint8_t mavlink_msg_command_long_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 31); -} - -/** - * @brief Get field command from command_long message - * - * @return Command ID, as defined by MAV_CMD enum. - */ -static inline uint16_t mavlink_msg_command_long_get_command(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 28); -} - -/** - * @brief Get field confirmation from command_long message - * - * @return 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - */ -static inline uint8_t mavlink_msg_command_long_get_confirmation(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field param1 from command_long message - * - * @return Parameter 1, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_long_get_param1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field param2 from command_long message - * - * @return Parameter 2, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_long_get_param2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field param3 from command_long message - * - * @return Parameter 3, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_long_get_param3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field param4 from command_long message - * - * @return Parameter 4, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_long_get_param4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field param5 from command_long message - * - * @return Parameter 5, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_long_get_param5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field param6 from command_long message - * - * @return Parameter 6, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_long_get_param6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field param7 from command_long message - * - * @return Parameter 7, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_long_get_param7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Decode a command_long message into a struct - * - * @param msg The message to decode - * @param command_long C-struct to decode the message contents into - */ -static inline void mavlink_msg_command_long_decode(const mavlink_message_t* msg, mavlink_command_long_t* command_long) -{ -#if MAVLINK_NEED_BYTE_SWAP - command_long->param1 = mavlink_msg_command_long_get_param1(msg); - command_long->param2 = mavlink_msg_command_long_get_param2(msg); - command_long->param3 = mavlink_msg_command_long_get_param3(msg); - command_long->param4 = mavlink_msg_command_long_get_param4(msg); - command_long->param5 = mavlink_msg_command_long_get_param5(msg); - command_long->param6 = mavlink_msg_command_long_get_param6(msg); - command_long->param7 = mavlink_msg_command_long_get_param7(msg); - command_long->command = mavlink_msg_command_long_get_command(msg); - command_long->target_system = mavlink_msg_command_long_get_target_system(msg); - command_long->target_component = mavlink_msg_command_long_get_target_component(msg); - command_long->confirmation = mavlink_msg_command_long_get_confirmation(msg); -#else - memcpy(command_long, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMMAND_LONG_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h deleted file mode 100644 index 640ffebf4..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE DATA_STREAM PACKING - -#define MAVLINK_MSG_ID_DATA_STREAM 67 - -typedef struct __mavlink_data_stream_t -{ - uint16_t message_rate; ///< The requested interval between two messages of this type - uint8_t stream_id; ///< The ID of the requested data stream - uint8_t on_off; ///< 1 stream is enabled, 0 stream is stopped. -} mavlink_data_stream_t; - -#define MAVLINK_MSG_ID_DATA_STREAM_LEN 4 -#define MAVLINK_MSG_ID_67_LEN 4 - -#define MAVLINK_MSG_ID_DATA_STREAM_CRC 21 -#define MAVLINK_MSG_ID_67_CRC 21 - - - -#define MAVLINK_MESSAGE_INFO_DATA_STREAM { \ - "DATA_STREAM", \ - 3, \ - { { "message_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_data_stream_t, message_rate) }, \ - { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_data_stream_t, stream_id) }, \ - { "on_off", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_data_stream_t, on_off) }, \ - } \ -} - - -/** - * @brief Pack a data_stream message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param stream_id The ID of the requested data stream - * @param message_rate The requested interval between two messages of this type - * @param on_off 1 stream is enabled, 0 stream is stopped. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t stream_id, uint16_t message_rate, uint8_t on_off) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA_STREAM_LEN]; - _mav_put_uint16_t(buf, 0, message_rate); - _mav_put_uint8_t(buf, 2, stream_id); - _mav_put_uint8_t(buf, 3, on_off); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_STREAM_LEN); -#else - mavlink_data_stream_t packet; - packet.message_rate = message_rate; - packet.stream_id = stream_id; - packet.on_off = on_off; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_STREAM_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_STREAM_LEN); -#endif -} - -/** - * @brief Pack a data_stream message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param stream_id The ID of the requested data stream - * @param message_rate The requested interval between two messages of this type - * @param on_off 1 stream is enabled, 0 stream is stopped. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data_stream_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t stream_id,uint16_t message_rate,uint8_t on_off) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA_STREAM_LEN]; - _mav_put_uint16_t(buf, 0, message_rate); - _mav_put_uint8_t(buf, 2, stream_id); - _mav_put_uint8_t(buf, 3, on_off); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_STREAM_LEN); -#else - mavlink_data_stream_t packet; - packet.message_rate = message_rate; - packet.stream_id = stream_id; - packet.on_off = on_off; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_STREAM_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_STREAM_LEN); -#endif -} - -/** - * @brief Encode a data_stream struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param data_stream C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data_stream_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data_stream_t* data_stream) -{ - return mavlink_msg_data_stream_pack(system_id, component_id, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off); -} - -/** - * @brief Encode a data_stream struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param data_stream C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data_stream_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data_stream_t* data_stream) -{ - return mavlink_msg_data_stream_pack_chan(system_id, component_id, chan, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off); -} - -/** - * @brief Send a data_stream message - * @param chan MAVLink channel to send the message - * - * @param stream_id The ID of the requested data stream - * @param message_rate The requested interval between two messages of this type - * @param on_off 1 stream is enabled, 0 stream is stopped. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data_stream_send(mavlink_channel_t chan, uint8_t stream_id, uint16_t message_rate, uint8_t on_off) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA_STREAM_LEN]; - _mav_put_uint16_t(buf, 0, message_rate); - _mav_put_uint8_t(buf, 2, stream_id); - _mav_put_uint8_t(buf, 3, on_off); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN); -#endif -#else - mavlink_data_stream_t packet; - packet.message_rate = message_rate; - packet.stream_id = stream_id; - packet.on_off = on_off; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)&packet, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)&packet, MAVLINK_MSG_ID_DATA_STREAM_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_data_stream_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint16_t message_rate, uint8_t on_off) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, message_rate); - _mav_put_uint8_t(buf, 2, stream_id); - _mav_put_uint8_t(buf, 3, on_off); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN); -#endif -#else - mavlink_data_stream_t *packet = (mavlink_data_stream_t *)msgbuf; - packet->message_rate = message_rate; - packet->stream_id = stream_id; - packet->on_off = on_off; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_DATA_STREAM_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE DATA_STREAM UNPACKING - - -/** - * @brief Get field stream_id from data_stream message - * - * @return The ID of the requested data stream - */ -static inline uint8_t mavlink_msg_data_stream_get_stream_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field message_rate from data_stream message - * - * @return The requested interval between two messages of this type - */ -static inline uint16_t mavlink_msg_data_stream_get_message_rate(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field on_off from data_stream message - * - * @return 1 stream is enabled, 0 stream is stopped. - */ -static inline uint8_t mavlink_msg_data_stream_get_on_off(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Decode a data_stream message into a struct - * - * @param msg The message to decode - * @param data_stream C-struct to decode the message contents into - */ -static inline void mavlink_msg_data_stream_decode(const mavlink_message_t* msg, mavlink_data_stream_t* data_stream) -{ -#if MAVLINK_NEED_BYTE_SWAP - data_stream->message_rate = mavlink_msg_data_stream_get_message_rate(msg); - data_stream->stream_id = mavlink_msg_data_stream_get_stream_id(msg); - data_stream->on_off = mavlink_msg_data_stream_get_on_off(msg); -#else - memcpy(data_stream, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA_STREAM_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h deleted file mode 100644 index d84a73709..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE DATA_TRANSMISSION_HANDSHAKE PACKING - -#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 130 - -typedef struct __mavlink_data_transmission_handshake_t -{ - uint32_t size; ///< total data size in bytes (set on ACK only) - uint16_t width; ///< Width of a matrix or image - uint16_t height; ///< Height of a matrix or image - uint16_t packets; ///< number of packets beeing sent (set on ACK only) - uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - uint8_t payload; ///< payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - uint8_t jpg_quality; ///< JPEG quality out of [1,100] -} mavlink_data_transmission_handshake_t; - -#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 13 -#define MAVLINK_MSG_ID_130_LEN 13 - -#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC 29 -#define MAVLINK_MSG_ID_130_CRC 29 - - - -#define MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE { \ - "DATA_TRANSMISSION_HANDSHAKE", \ - 7, \ - { { "size", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_data_transmission_handshake_t, size) }, \ - { "width", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_data_transmission_handshake_t, width) }, \ - { "height", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_data_transmission_handshake_t, height) }, \ - { "packets", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_data_transmission_handshake_t, packets) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_data_transmission_handshake_t, type) }, \ - { "payload", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_data_transmission_handshake_t, payload) }, \ - { "jpg_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_data_transmission_handshake_t, jpg_quality) }, \ - } \ -} - - -/** - * @brief Pack a data_transmission_handshake message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - * @param size total data size in bytes (set on ACK only) - * @param width Width of a matrix or image - * @param height Height of a matrix or image - * @param packets number of packets beeing sent (set on ACK only) - * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - * @param jpg_quality JPEG quality out of [1,100] - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN]; - _mav_put_uint32_t(buf, 0, size); - _mav_put_uint16_t(buf, 4, width); - _mav_put_uint16_t(buf, 6, height); - _mav_put_uint16_t(buf, 8, packets); - _mav_put_uint8_t(buf, 10, type); - _mav_put_uint8_t(buf, 11, payload); - _mav_put_uint8_t(buf, 12, jpg_quality); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); -#else - mavlink_data_transmission_handshake_t packet; - packet.size = size; - packet.width = width; - packet.height = height; - packet.packets = packets; - packet.type = type; - packet.payload = payload; - packet.jpg_quality = jpg_quality; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); -#endif -} - -/** - * @brief Pack a data_transmission_handshake message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - * @param size total data size in bytes (set on ACK only) - * @param width Width of a matrix or image - * @param height Height of a matrix or image - * @param packets number of packets beeing sent (set on ACK only) - * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - * @param jpg_quality JPEG quality out of [1,100] - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint32_t size,uint16_t width,uint16_t height,uint16_t packets,uint8_t payload,uint8_t jpg_quality) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN]; - _mav_put_uint32_t(buf, 0, size); - _mav_put_uint16_t(buf, 4, width); - _mav_put_uint16_t(buf, 6, height); - _mav_put_uint16_t(buf, 8, packets); - _mav_put_uint8_t(buf, 10, type); - _mav_put_uint8_t(buf, 11, payload); - _mav_put_uint8_t(buf, 12, jpg_quality); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); -#else - mavlink_data_transmission_handshake_t packet; - packet.size = size; - packet.width = width; - packet.height = height; - packet.packets = packets; - packet.type = type; - packet.payload = payload; - packet.jpg_quality = jpg_quality; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); -#endif -} - -/** - * @brief Encode a data_transmission_handshake struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param data_transmission_handshake C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data_transmission_handshake_t* data_transmission_handshake) -{ - return mavlink_msg_data_transmission_handshake_pack(system_id, component_id, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->width, data_transmission_handshake->height, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); -} - -/** - * @brief Encode a data_transmission_handshake struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param data_transmission_handshake C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data_transmission_handshake_t* data_transmission_handshake) -{ - return mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, chan, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->width, data_transmission_handshake->height, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); -} - -/** - * @brief Send a data_transmission_handshake message - * @param chan MAVLink channel to send the message - * - * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - * @param size total data size in bytes (set on ACK only) - * @param width Width of a matrix or image - * @param height Height of a matrix or image - * @param packets number of packets beeing sent (set on ACK only) - * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - * @param jpg_quality JPEG quality out of [1,100] - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN]; - _mav_put_uint32_t(buf, 0, size); - _mav_put_uint16_t(buf, 4, width); - _mav_put_uint16_t(buf, 6, height); - _mav_put_uint16_t(buf, 8, packets); - _mav_put_uint8_t(buf, 10, type); - _mav_put_uint8_t(buf, 11, payload); - _mav_put_uint8_t(buf, 12, jpg_quality); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); -#endif -#else - mavlink_data_transmission_handshake_t packet; - packet.size = size; - packet.width = width; - packet.height = height; - packet.packets = packets; - packet.type = type; - packet.payload = payload; - packet.jpg_quality = jpg_quality; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)&packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)&packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_data_transmission_handshake_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, size); - _mav_put_uint16_t(buf, 4, width); - _mav_put_uint16_t(buf, 6, height); - _mav_put_uint16_t(buf, 8, packets); - _mav_put_uint8_t(buf, 10, type); - _mav_put_uint8_t(buf, 11, payload); - _mav_put_uint8_t(buf, 12, jpg_quality); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); -#endif -#else - mavlink_data_transmission_handshake_t *packet = (mavlink_data_transmission_handshake_t *)msgbuf; - packet->size = size; - packet->width = width; - packet->height = height; - packet->packets = packets; - packet->type = type; - packet->payload = payload; - packet->jpg_quality = jpg_quality; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE DATA_TRANSMISSION_HANDSHAKE UNPACKING - - -/** - * @brief Get field type from data_transmission_handshake message - * - * @return type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - */ -static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field size from data_transmission_handshake message - * - * @return total data size in bytes (set on ACK only) - */ -static inline uint32_t mavlink_msg_data_transmission_handshake_get_size(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field width from data_transmission_handshake message - * - * @return Width of a matrix or image - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_get_width(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field height from data_transmission_handshake message - * - * @return Height of a matrix or image - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_get_height(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field packets from data_transmission_handshake message - * - * @return number of packets beeing sent (set on ACK only) - */ -static inline uint16_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field payload from data_transmission_handshake message - * - * @return payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - */ -static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field jpg_quality from data_transmission_handshake message - * - * @return JPEG quality out of [1,100] - */ -static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Decode a data_transmission_handshake message into a struct - * - * @param msg The message to decode - * @param data_transmission_handshake C-struct to decode the message contents into - */ -static inline void mavlink_msg_data_transmission_handshake_decode(const mavlink_message_t* msg, mavlink_data_transmission_handshake_t* data_transmission_handshake) -{ -#if MAVLINK_NEED_BYTE_SWAP - data_transmission_handshake->size = mavlink_msg_data_transmission_handshake_get_size(msg); - data_transmission_handshake->width = mavlink_msg_data_transmission_handshake_get_width(msg); - data_transmission_handshake->height = mavlink_msg_data_transmission_handshake_get_height(msg); - data_transmission_handshake->packets = mavlink_msg_data_transmission_handshake_get_packets(msg); - data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg); - data_transmission_handshake->payload = mavlink_msg_data_transmission_handshake_get_payload(msg); - data_transmission_handshake->jpg_quality = mavlink_msg_data_transmission_handshake_get_jpg_quality(msg); -#else - memcpy(data_transmission_handshake, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h deleted file mode 100644 index 2102af862..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE DEBUG PACKING - -#define MAVLINK_MSG_ID_DEBUG 254 - -typedef struct __mavlink_debug_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - float value; ///< DEBUG value - uint8_t ind; ///< index of debug variable -} mavlink_debug_t; - -#define MAVLINK_MSG_ID_DEBUG_LEN 9 -#define MAVLINK_MSG_ID_254_LEN 9 - -#define MAVLINK_MSG_ID_DEBUG_CRC 46 -#define MAVLINK_MSG_ID_254_CRC 46 - - - -#define MAVLINK_MESSAGE_INFO_DEBUG { \ - "DEBUG", \ - 3, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_debug_t, time_boot_ms) }, \ - { "value", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_debug_t, value) }, \ - { "ind", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_debug_t, ind) }, \ - } \ -} - - -/** - * @brief Pack a debug message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param ind index of debug variable - * @param value DEBUG value - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t ind, float value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DEBUG_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, value); - _mav_put_uint8_t(buf, 8, ind); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_LEN); -#else - mavlink_debug_t packet; - packet.time_boot_ms = time_boot_ms; - packet.value = value; - packet.ind = ind; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DEBUG; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_LEN); -#endif -} - -/** - * @brief Pack a debug message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param ind index of debug variable - * @param value DEBUG value - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t ind,float value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DEBUG_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, value); - _mav_put_uint8_t(buf, 8, ind); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_LEN); -#else - mavlink_debug_t packet; - packet.time_boot_ms = time_boot_ms; - packet.value = value; - packet.ind = ind; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DEBUG; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_LEN); -#endif -} - -/** - * @brief Encode a debug struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param debug C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_debug_t* debug) -{ - return mavlink_msg_debug_pack(system_id, component_id, msg, debug->time_boot_ms, debug->ind, debug->value); -} - -/** - * @brief Encode a debug struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param debug C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_debug_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_t* debug) -{ - return mavlink_msg_debug_pack_chan(system_id, component_id, chan, msg, debug->time_boot_ms, debug->ind, debug->value); -} - -/** - * @brief Send a debug message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param ind index of debug variable - * @param value DEBUG value - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t ind, float value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DEBUG_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, value); - _mav_put_uint8_t(buf, 8, ind); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN); -#endif -#else - mavlink_debug_t packet; - packet.time_boot_ms = time_boot_ms; - packet.value = value; - packet.ind = ind; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_DEBUG_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_debug_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t ind, float value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, value); - _mav_put_uint8_t(buf, 8, ind); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN); -#endif -#else - mavlink_debug_t *packet = (mavlink_debug_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->value = value; - packet->ind = ind; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE DEBUG UNPACKING - - -/** - * @brief Get field time_boot_ms from debug message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_debug_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field ind from debug message - * - * @return index of debug variable - */ -static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field value from debug message - * - * @return DEBUG value - */ -static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Decode a debug message into a struct - * - * @param msg The message to decode - * @param debug C-struct to decode the message contents into - */ -static inline void mavlink_msg_debug_decode(const mavlink_message_t* msg, mavlink_debug_t* debug) -{ -#if MAVLINK_NEED_BYTE_SWAP - debug->time_boot_ms = mavlink_msg_debug_get_time_boot_ms(msg); - debug->value = mavlink_msg_debug_get_value(msg); - debug->ind = mavlink_msg_debug_get_ind(msg); -#else - memcpy(debug, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DEBUG_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h deleted file mode 100644 index 67c339e89..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h +++ /dev/null @@ -1,297 +0,0 @@ -// MESSAGE DEBUG_VECT PACKING - -#define MAVLINK_MSG_ID_DEBUG_VECT 250 - -typedef struct __mavlink_debug_vect_t -{ - uint64_t time_usec; ///< Timestamp - float x; ///< x - float y; ///< y - float z; ///< z - char name[10]; ///< Name -} mavlink_debug_vect_t; - -#define MAVLINK_MSG_ID_DEBUG_VECT_LEN 30 -#define MAVLINK_MSG_ID_250_LEN 30 - -#define MAVLINK_MSG_ID_DEBUG_VECT_CRC 49 -#define MAVLINK_MSG_ID_250_CRC 49 - -#define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10 - -#define MAVLINK_MESSAGE_INFO_DEBUG_VECT { \ - "DEBUG_VECT", \ - 5, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_debug_vect_t, time_usec) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_debug_vect_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_debug_vect_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_debug_vect_t, z) }, \ - { "name", NULL, MAVLINK_TYPE_CHAR, 10, 20, offsetof(mavlink_debug_vect_t, name) }, \ - } \ -} - - -/** - * @brief Pack a debug_vect message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param name Name - * @param time_usec Timestamp - * @param x x - * @param y y - * @param z z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const char *name, uint64_t time_usec, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DEBUG_VECT_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_char_array(buf, 20, name, 10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN); -#else - mavlink_debug_vect_t packet; - packet.time_usec = time_usec; - packet.x = x; - packet.y = y; - packet.z = z; - mav_array_memcpy(packet.name, name, sizeof(char)*10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_LEN); -#endif -} - -/** - * @brief Pack a debug_vect message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param name Name - * @param time_usec Timestamp - * @param x x - * @param y y - * @param z z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const char *name,uint64_t time_usec,float x,float y,float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DEBUG_VECT_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_char_array(buf, 20, name, 10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN); -#else - mavlink_debug_vect_t packet; - packet.time_usec = time_usec; - packet.x = x; - packet.y = y; - packet.z = z; - mav_array_memcpy(packet.name, name, sizeof(char)*10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN); -#endif -} - -/** - * @brief Encode a debug_vect struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param debug_vect C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_debug_vect_t* debug_vect) -{ - return mavlink_msg_debug_vect_pack(system_id, component_id, msg, debug_vect->name, debug_vect->time_usec, debug_vect->x, debug_vect->y, debug_vect->z); -} - -/** - * @brief Encode a debug_vect struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param debug_vect C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_debug_vect_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_vect_t* debug_vect) -{ - return mavlink_msg_debug_vect_pack_chan(system_id, component_id, chan, msg, debug_vect->name, debug_vect->time_usec, debug_vect->x, debug_vect->y, debug_vect->z); -} - -/** - * @brief Send a debug_vect message - * @param chan MAVLink channel to send the message - * - * @param name Name - * @param time_usec Timestamp - * @param x x - * @param y y - * @param z z - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char *name, uint64_t time_usec, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DEBUG_VECT_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_char_array(buf, 20, name, 10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN); -#endif -#else - mavlink_debug_vect_t packet; - packet.time_usec = time_usec; - packet.x = x; - packet.y = y; - packet.z = z; - mav_array_memcpy(packet.name, name, sizeof(char)*10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_DEBUG_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_debug_vect_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *name, uint64_t time_usec, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_char_array(buf, 20, name, 10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN); -#endif -#else - mavlink_debug_vect_t *packet = (mavlink_debug_vect_t *)msgbuf; - packet->time_usec = time_usec; - packet->x = x; - packet->y = y; - packet->z = z; - mav_array_memcpy(packet->name, name, sizeof(char)*10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE DEBUG_VECT UNPACKING - - -/** - * @brief Get field name from debug_vect message - * - * @return Name - */ -static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, char *name) -{ - return _MAV_RETURN_char_array(msg, name, 10, 20); -} - -/** - * @brief Get field time_usec from debug_vect message - * - * @return Timestamp - */ -static inline uint64_t mavlink_msg_debug_vect_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field x from debug_vect message - * - * @return x - */ -static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field y from debug_vect message - * - * @return y - */ -static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field z from debug_vect message - * - * @return z - */ -static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a debug_vect message into a struct - * - * @param msg The message to decode - * @param debug_vect C-struct to decode the message contents into - */ -static inline void mavlink_msg_debug_vect_decode(const mavlink_message_t* msg, mavlink_debug_vect_t* debug_vect) -{ -#if MAVLINK_NEED_BYTE_SWAP - debug_vect->time_usec = mavlink_msg_debug_vect_get_time_usec(msg); - debug_vect->x = mavlink_msg_debug_vect_get_x(msg); - debug_vect->y = mavlink_msg_debug_vect_get_y(msg); - debug_vect->z = mavlink_msg_debug_vect_get_z(msg); - mavlink_msg_debug_vect_get_name(msg, debug_vect->name); -#else - memcpy(debug_vect, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DEBUG_VECT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h deleted file mode 100644 index 39b384396..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h +++ /dev/null @@ -1,377 +0,0 @@ -// MESSAGE DISTANCE_SENSOR PACKING - -#define MAVLINK_MSG_ID_DISTANCE_SENSOR 132 - -typedef struct __mavlink_distance_sensor_t -{ - uint32_t time_boot_ms; ///< Time since system boot - uint16_t min_distance; ///< Minimum distance the sensor can measure in centimeters - uint16_t max_distance; ///< Maximum distance the sensor can measure in centimeters - uint16_t current_distance; ///< Current distance reading - uint8_t type; ///< Type from MAV_DISTANCE_SENSOR enum. - uint8_t id; ///< Onboard ID of the sensor - uint8_t orientation; ///< Direction the sensor faces from FIXME enum. - uint8_t covariance; ///< Measurement covariance in centimeters, 0 for unknown / invalid readings -} mavlink_distance_sensor_t; - -#define MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN 14 -#define MAVLINK_MSG_ID_132_LEN 14 - -#define MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC 85 -#define MAVLINK_MSG_ID_132_CRC 85 - - - -#define MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR { \ - "DISTANCE_SENSOR", \ - 8, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_distance_sensor_t, time_boot_ms) }, \ - { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_distance_sensor_t, min_distance) }, \ - { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_distance_sensor_t, max_distance) }, \ - { "current_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_distance_sensor_t, current_distance) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_distance_sensor_t, type) }, \ - { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_distance_sensor_t, id) }, \ - { "orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_distance_sensor_t, orientation) }, \ - { "covariance", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_distance_sensor_t, covariance) }, \ - } \ -} - - -/** - * @brief Pack a distance_sensor message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Time since system boot - * @param type Type from MAV_DISTANCE_SENSOR enum. - * @param id Onboard ID of the sensor - * @param orientation Direction the sensor faces from FIXME enum. - * @param min_distance Minimum distance the sensor can measure in centimeters - * @param max_distance Maximum distance the sensor can measure in centimeters - * @param current_distance Current distance reading - * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, min_distance); - _mav_put_uint16_t(buf, 6, max_distance); - _mav_put_uint16_t(buf, 8, current_distance); - _mav_put_uint8_t(buf, 10, type); - _mav_put_uint8_t(buf, 11, id); - _mav_put_uint8_t(buf, 12, orientation); - _mav_put_uint8_t(buf, 13, covariance); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); -#else - mavlink_distance_sensor_t packet; - packet.time_boot_ms = time_boot_ms; - packet.min_distance = min_distance; - packet.max_distance = max_distance; - packet.current_distance = current_distance; - packet.type = type; - packet.id = id; - packet.orientation = orientation; - packet.covariance = covariance; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); -#endif -} - -/** - * @brief Pack a distance_sensor message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Time since system boot - * @param type Type from MAV_DISTANCE_SENSOR enum. - * @param id Onboard ID of the sensor - * @param orientation Direction the sensor faces from FIXME enum. - * @param min_distance Minimum distance the sensor can measure in centimeters - * @param max_distance Maximum distance the sensor can measure in centimeters - * @param current_distance Current distance reading - * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t type,uint8_t id,uint8_t orientation,uint16_t min_distance,uint16_t max_distance,uint16_t current_distance,uint8_t covariance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, min_distance); - _mav_put_uint16_t(buf, 6, max_distance); - _mav_put_uint16_t(buf, 8, current_distance); - _mav_put_uint8_t(buf, 10, type); - _mav_put_uint8_t(buf, 11, id); - _mav_put_uint8_t(buf, 12, orientation); - _mav_put_uint8_t(buf, 13, covariance); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); -#else - mavlink_distance_sensor_t packet; - packet.time_boot_ms = time_boot_ms; - packet.min_distance = min_distance; - packet.max_distance = max_distance; - packet.current_distance = current_distance; - packet.type = type; - packet.id = id; - packet.orientation = orientation; - packet.covariance = covariance; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); -#endif -} - -/** - * @brief Encode a distance_sensor struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param distance_sensor C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_distance_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) -{ - return mavlink_msg_distance_sensor_pack(system_id, component_id, msg, distance_sensor->time_boot_ms, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->covariance); -} - -/** - * @brief Encode a distance_sensor struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param distance_sensor C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) -{ - return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->covariance); -} - -/** - * @brief Send a distance_sensor message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Time since system boot - * @param type Type from MAV_DISTANCE_SENSOR enum. - * @param id Onboard ID of the sensor - * @param orientation Direction the sensor faces from FIXME enum. - * @param min_distance Minimum distance the sensor can measure in centimeters - * @param max_distance Maximum distance the sensor can measure in centimeters - * @param current_distance Current distance reading - * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, min_distance); - _mav_put_uint16_t(buf, 6, max_distance); - _mav_put_uint16_t(buf, 8, current_distance); - _mav_put_uint8_t(buf, 10, type); - _mav_put_uint8_t(buf, 11, id); - _mav_put_uint8_t(buf, 12, orientation); - _mav_put_uint8_t(buf, 13, covariance); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); -#endif -#else - mavlink_distance_sensor_t packet; - packet.time_boot_ms = time_boot_ms; - packet.min_distance = min_distance; - packet.max_distance = max_distance; - packet.current_distance = current_distance; - packet.type = type; - packet.id = id; - packet.orientation = orientation; - packet.covariance = covariance; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, min_distance); - _mav_put_uint16_t(buf, 6, max_distance); - _mav_put_uint16_t(buf, 8, current_distance); - _mav_put_uint8_t(buf, 10, type); - _mav_put_uint8_t(buf, 11, id); - _mav_put_uint8_t(buf, 12, orientation); - _mav_put_uint8_t(buf, 13, covariance); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); -#endif -#else - mavlink_distance_sensor_t *packet = (mavlink_distance_sensor_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->min_distance = min_distance; - packet->max_distance = max_distance; - packet->current_distance = current_distance; - packet->type = type; - packet->id = id; - packet->orientation = orientation; - packet->covariance = covariance; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE DISTANCE_SENSOR UNPACKING - - -/** - * @brief Get field time_boot_ms from distance_sensor message - * - * @return Time since system boot - */ -static inline uint32_t mavlink_msg_distance_sensor_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field type from distance_sensor message - * - * @return Type from MAV_DISTANCE_SENSOR enum. - */ -static inline uint8_t mavlink_msg_distance_sensor_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field id from distance_sensor message - * - * @return Onboard ID of the sensor - */ -static inline uint8_t mavlink_msg_distance_sensor_get_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field orientation from distance_sensor message - * - * @return Direction the sensor faces from FIXME enum. - */ -static inline uint8_t mavlink_msg_distance_sensor_get_orientation(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field min_distance from distance_sensor message - * - * @return Minimum distance the sensor can measure in centimeters - */ -static inline uint16_t mavlink_msg_distance_sensor_get_min_distance(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field max_distance from distance_sensor message - * - * @return Maximum distance the sensor can measure in centimeters - */ -static inline uint16_t mavlink_msg_distance_sensor_get_max_distance(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field current_distance from distance_sensor message - * - * @return Current distance reading - */ -static inline uint16_t mavlink_msg_distance_sensor_get_current_distance(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field covariance from distance_sensor message - * - * @return Measurement covariance in centimeters, 0 for unknown / invalid readings - */ -static inline uint8_t mavlink_msg_distance_sensor_get_covariance(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 13); -} - -/** - * @brief Decode a distance_sensor message into a struct - * - * @param msg The message to decode - * @param distance_sensor C-struct to decode the message contents into - */ -static inline void mavlink_msg_distance_sensor_decode(const mavlink_message_t* msg, mavlink_distance_sensor_t* distance_sensor) -{ -#if MAVLINK_NEED_BYTE_SWAP - distance_sensor->time_boot_ms = mavlink_msg_distance_sensor_get_time_boot_ms(msg); - distance_sensor->min_distance = mavlink_msg_distance_sensor_get_min_distance(msg); - distance_sensor->max_distance = mavlink_msg_distance_sensor_get_max_distance(msg); - distance_sensor->current_distance = mavlink_msg_distance_sensor_get_current_distance(msg); - distance_sensor->type = mavlink_msg_distance_sensor_get_type(msg); - distance_sensor->id = mavlink_msg_distance_sensor_get_id(msg); - distance_sensor->orientation = mavlink_msg_distance_sensor_get_orientation(msg); - distance_sensor->covariance = mavlink_msg_distance_sensor_get_covariance(msg); -#else - memcpy(distance_sensor, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h deleted file mode 100644 index dacd7c9f4..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h +++ /dev/null @@ -1,225 +0,0 @@ -// MESSAGE ENCAPSULATED_DATA PACKING - -#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 131 - -typedef struct __mavlink_encapsulated_data_t -{ - uint16_t seqnr; ///< sequence number (starting with 0 on every transmission) - uint8_t data[253]; ///< image data bytes -} mavlink_encapsulated_data_t; - -#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255 -#define MAVLINK_MSG_ID_131_LEN 255 - -#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC 223 -#define MAVLINK_MSG_ID_131_CRC 223 - -#define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253 - -#define MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA { \ - "ENCAPSULATED_DATA", \ - 2, \ - { { "seqnr", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_encapsulated_data_t, seqnr) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 253, 2, offsetof(mavlink_encapsulated_data_t, data) }, \ - } \ -} - - -/** - * @brief Pack a encapsulated_data message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param seqnr sequence number (starting with 0 on every transmission) - * @param data image data bytes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t seqnr, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN]; - _mav_put_uint16_t(buf, 0, seqnr); - _mav_put_uint8_t_array(buf, 2, data, 253); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); -#else - mavlink_encapsulated_data_t packet; - packet.seqnr = seqnr; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); -#endif -} - -/** - * @brief Pack a encapsulated_data message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param seqnr sequence number (starting with 0 on every transmission) - * @param data image data bytes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t seqnr,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN]; - _mav_put_uint16_t(buf, 0, seqnr); - _mav_put_uint8_t_array(buf, 2, data, 253); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); -#else - mavlink_encapsulated_data_t packet; - packet.seqnr = seqnr; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); -#endif -} - -/** - * @brief Encode a encapsulated_data struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param encapsulated_data C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data) -{ - return mavlink_msg_encapsulated_data_pack(system_id, component_id, msg, encapsulated_data->seqnr, encapsulated_data->data); -} - -/** - * @brief Encode a encapsulated_data struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param encapsulated_data C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_encapsulated_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data) -{ - return mavlink_msg_encapsulated_data_pack_chan(system_id, component_id, chan, msg, encapsulated_data->seqnr, encapsulated_data->data); -} - -/** - * @brief Send a encapsulated_data message - * @param chan MAVLink channel to send the message - * - * @param seqnr sequence number (starting with 0 on every transmission) - * @param data image data bytes - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN]; - _mav_put_uint16_t(buf, 0, seqnr); - _mav_put_uint8_t_array(buf, 2, data, 253); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); -#endif -#else - mavlink_encapsulated_data_t packet; - packet.seqnr = seqnr; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)&packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)&packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_encapsulated_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seqnr, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, seqnr); - _mav_put_uint8_t_array(buf, 2, data, 253); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); -#endif -#else - mavlink_encapsulated_data_t *packet = (mavlink_encapsulated_data_t *)msgbuf; - packet->seqnr = seqnr; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*253); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE ENCAPSULATED_DATA UNPACKING - - -/** - * @brief Get field seqnr from encapsulated_data message - * - * @return sequence number (starting with 0 on every transmission) - */ -static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field data from encapsulated_data message - * - * @return image data bytes - */ -static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 253, 2); -} - -/** - * @brief Decode a encapsulated_data message into a struct - * - * @param msg The message to decode - * @param encapsulated_data C-struct to decode the message contents into - */ -static inline void mavlink_msg_encapsulated_data_decode(const mavlink_message_t* msg, mavlink_encapsulated_data_t* encapsulated_data) -{ -#if MAVLINK_NEED_BYTE_SWAP - encapsulated_data->seqnr = mavlink_msg_encapsulated_data_get_seqnr(msg); - mavlink_msg_encapsulated_data_get_data(msg, encapsulated_data->data); -#else - memcpy(encapsulated_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h deleted file mode 100644 index 81c7031ce..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h +++ /dev/null @@ -1,249 +0,0 @@ -// MESSAGE FILE_TRANSFER_DIR_LIST PACKING - -#define MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST 111 - -typedef struct __mavlink_file_transfer_dir_list_t -{ - uint64_t transfer_uid; ///< Unique transfer ID - char dir_path[240]; ///< Directory path to list - uint8_t flags; ///< RESERVED -} mavlink_file_transfer_dir_list_t; - -#define MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN 249 -#define MAVLINK_MSG_ID_111_LEN 249 - -#define MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC 93 -#define MAVLINK_MSG_ID_111_CRC 93 - -#define MAVLINK_MSG_FILE_TRANSFER_DIR_LIST_FIELD_DIR_PATH_LEN 240 - -#define MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST { \ - "FILE_TRANSFER_DIR_LIST", \ - 3, \ - { { "transfer_uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_file_transfer_dir_list_t, transfer_uid) }, \ - { "dir_path", NULL, MAVLINK_TYPE_CHAR, 240, 8, offsetof(mavlink_file_transfer_dir_list_t, dir_path) }, \ - { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 248, offsetof(mavlink_file_transfer_dir_list_t, flags) }, \ - } \ -} - - -/** - * @brief Pack a file_transfer_dir_list message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param transfer_uid Unique transfer ID - * @param dir_path Directory path to list - * @param flags RESERVED - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_file_transfer_dir_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t transfer_uid, const char *dir_path, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 248, flags); - _mav_put_char_array(buf, 8, dir_path, 240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#else - mavlink_file_transfer_dir_list_t packet; - packet.transfer_uid = transfer_uid; - packet.flags = flags; - mav_array_memcpy(packet.dir_path, dir_path, sizeof(char)*240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif -} - -/** - * @brief Pack a file_transfer_dir_list message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param transfer_uid Unique transfer ID - * @param dir_path Directory path to list - * @param flags RESERVED - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_file_transfer_dir_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t transfer_uid,const char *dir_path,uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 248, flags); - _mav_put_char_array(buf, 8, dir_path, 240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#else - mavlink_file_transfer_dir_list_t packet; - packet.transfer_uid = transfer_uid; - packet.flags = flags; - mav_array_memcpy(packet.dir_path, dir_path, sizeof(char)*240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif -} - -/** - * @brief Encode a file_transfer_dir_list struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param file_transfer_dir_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_file_transfer_dir_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_file_transfer_dir_list_t* file_transfer_dir_list) -{ - return mavlink_msg_file_transfer_dir_list_pack(system_id, component_id, msg, file_transfer_dir_list->transfer_uid, file_transfer_dir_list->dir_path, file_transfer_dir_list->flags); -} - -/** - * @brief Encode a file_transfer_dir_list struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param file_transfer_dir_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_file_transfer_dir_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_dir_list_t* file_transfer_dir_list) -{ - return mavlink_msg_file_transfer_dir_list_pack_chan(system_id, component_id, chan, msg, file_transfer_dir_list->transfer_uid, file_transfer_dir_list->dir_path, file_transfer_dir_list->flags); -} - -/** - * @brief Send a file_transfer_dir_list message - * @param chan MAVLink channel to send the message - * - * @param transfer_uid Unique transfer ID - * @param dir_path Directory path to list - * @param flags RESERVED - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_file_transfer_dir_list_send(mavlink_channel_t chan, uint64_t transfer_uid, const char *dir_path, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 248, flags); - _mav_put_char_array(buf, 8, dir_path, 240); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif -#else - mavlink_file_transfer_dir_list_t packet; - packet.transfer_uid = transfer_uid; - packet.flags = flags; - mav_array_memcpy(packet.dir_path, dir_path, sizeof(char)*240); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_file_transfer_dir_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t transfer_uid, const char *dir_path, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 248, flags); - _mav_put_char_array(buf, 8, dir_path, 240); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif -#else - mavlink_file_transfer_dir_list_t *packet = (mavlink_file_transfer_dir_list_t *)msgbuf; - packet->transfer_uid = transfer_uid; - packet->flags = flags; - mav_array_memcpy(packet->dir_path, dir_path, sizeof(char)*240); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FILE_TRANSFER_DIR_LIST UNPACKING - - -/** - * @brief Get field transfer_uid from file_transfer_dir_list message - * - * @return Unique transfer ID - */ -static inline uint64_t mavlink_msg_file_transfer_dir_list_get_transfer_uid(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field dir_path from file_transfer_dir_list message - * - * @return Directory path to list - */ -static inline uint16_t mavlink_msg_file_transfer_dir_list_get_dir_path(const mavlink_message_t* msg, char *dir_path) -{ - return _MAV_RETURN_char_array(msg, dir_path, 240, 8); -} - -/** - * @brief Get field flags from file_transfer_dir_list message - * - * @return RESERVED - */ -static inline uint8_t mavlink_msg_file_transfer_dir_list_get_flags(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 248); -} - -/** - * @brief Decode a file_transfer_dir_list message into a struct - * - * @param msg The message to decode - * @param file_transfer_dir_list C-struct to decode the message contents into - */ -static inline void mavlink_msg_file_transfer_dir_list_decode(const mavlink_message_t* msg, mavlink_file_transfer_dir_list_t* file_transfer_dir_list) -{ -#if MAVLINK_NEED_BYTE_SWAP - file_transfer_dir_list->transfer_uid = mavlink_msg_file_transfer_dir_list_get_transfer_uid(msg); - mavlink_msg_file_transfer_dir_list_get_dir_path(msg, file_transfer_dir_list->dir_path); - file_transfer_dir_list->flags = mavlink_msg_file_transfer_dir_list_get_flags(msg); -#else - memcpy(file_transfer_dir_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h deleted file mode 100644 index 04981fa85..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE FILE_TRANSFER_RES PACKING - -#define MAVLINK_MSG_ID_FILE_TRANSFER_RES 112 - -typedef struct __mavlink_file_transfer_res_t -{ - uint64_t transfer_uid; ///< Unique transfer ID - uint8_t result; ///< 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device -} mavlink_file_transfer_res_t; - -#define MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN 9 -#define MAVLINK_MSG_ID_112_LEN 9 - -#define MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC 124 -#define MAVLINK_MSG_ID_112_CRC 124 - - - -#define MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES { \ - "FILE_TRANSFER_RES", \ - 2, \ - { { "transfer_uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_file_transfer_res_t, transfer_uid) }, \ - { "result", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_file_transfer_res_t, result) }, \ - } \ -} - - -/** - * @brief Pack a file_transfer_res message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param transfer_uid Unique transfer ID - * @param result 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_file_transfer_res_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t transfer_uid, uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 8, result); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#else - mavlink_file_transfer_res_t packet; - packet.transfer_uid = transfer_uid; - packet.result = result; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_RES; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif -} - -/** - * @brief Pack a file_transfer_res message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param transfer_uid Unique transfer ID - * @param result 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_file_transfer_res_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t transfer_uid,uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 8, result); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#else - mavlink_file_transfer_res_t packet; - packet.transfer_uid = transfer_uid; - packet.result = result; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_RES; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif -} - -/** - * @brief Encode a file_transfer_res struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param file_transfer_res C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_file_transfer_res_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_file_transfer_res_t* file_transfer_res) -{ - return mavlink_msg_file_transfer_res_pack(system_id, component_id, msg, file_transfer_res->transfer_uid, file_transfer_res->result); -} - -/** - * @brief Encode a file_transfer_res struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param file_transfer_res C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_file_transfer_res_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_res_t* file_transfer_res) -{ - return mavlink_msg_file_transfer_res_pack_chan(system_id, component_id, chan, msg, file_transfer_res->transfer_uid, file_transfer_res->result); -} - -/** - * @brief Send a file_transfer_res message - * @param chan MAVLink channel to send the message - * - * @param transfer_uid Unique transfer ID - * @param result 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_file_transfer_res_send(mavlink_channel_t chan, uint64_t transfer_uid, uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 8, result); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif -#else - mavlink_file_transfer_res_t packet; - packet.transfer_uid = transfer_uid; - packet.result = result; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_file_transfer_res_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t transfer_uid, uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint8_t(buf, 8, result); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif -#else - mavlink_file_transfer_res_t *packet = (mavlink_file_transfer_res_t *)msgbuf; - packet->transfer_uid = transfer_uid; - packet->result = result; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FILE_TRANSFER_RES UNPACKING - - -/** - * @brief Get field transfer_uid from file_transfer_res message - * - * @return Unique transfer ID - */ -static inline uint64_t mavlink_msg_file_transfer_res_get_transfer_uid(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field result from file_transfer_res message - * - * @return 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device - */ -static inline uint8_t mavlink_msg_file_transfer_res_get_result(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Decode a file_transfer_res message into a struct - * - * @param msg The message to decode - * @param file_transfer_res C-struct to decode the message contents into - */ -static inline void mavlink_msg_file_transfer_res_decode(const mavlink_message_t* msg, mavlink_file_transfer_res_t* file_transfer_res) -{ -#if MAVLINK_NEED_BYTE_SWAP - file_transfer_res->transfer_uid = mavlink_msg_file_transfer_res_get_transfer_uid(msg); - file_transfer_res->result = mavlink_msg_file_transfer_res_get_result(msg); -#else - memcpy(file_transfer_res, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h deleted file mode 100644 index 7b1fba519..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h +++ /dev/null @@ -1,297 +0,0 @@ -// MESSAGE FILE_TRANSFER_START PACKING - -#define MAVLINK_MSG_ID_FILE_TRANSFER_START 110 - -typedef struct __mavlink_file_transfer_start_t -{ - uint64_t transfer_uid; ///< Unique transfer ID - uint32_t file_size; ///< File size in bytes - char dest_path[240]; ///< Destination path - uint8_t direction; ///< Transfer direction: 0: from requester, 1: to requester - uint8_t flags; ///< RESERVED -} mavlink_file_transfer_start_t; - -#define MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN 254 -#define MAVLINK_MSG_ID_110_LEN 254 - -#define MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC 235 -#define MAVLINK_MSG_ID_110_CRC 235 - -#define MAVLINK_MSG_FILE_TRANSFER_START_FIELD_DEST_PATH_LEN 240 - -#define MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START { \ - "FILE_TRANSFER_START", \ - 5, \ - { { "transfer_uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_file_transfer_start_t, transfer_uid) }, \ - { "file_size", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_file_transfer_start_t, file_size) }, \ - { "dest_path", NULL, MAVLINK_TYPE_CHAR, 240, 12, offsetof(mavlink_file_transfer_start_t, dest_path) }, \ - { "direction", NULL, MAVLINK_TYPE_UINT8_T, 0, 252, offsetof(mavlink_file_transfer_start_t, direction) }, \ - { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 253, offsetof(mavlink_file_transfer_start_t, flags) }, \ - } \ -} - - -/** - * @brief Pack a file_transfer_start message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param transfer_uid Unique transfer ID - * @param dest_path Destination path - * @param direction Transfer direction: 0: from requester, 1: to requester - * @param file_size File size in bytes - * @param flags RESERVED - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_file_transfer_start_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t transfer_uid, const char *dest_path, uint8_t direction, uint32_t file_size, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint32_t(buf, 8, file_size); - _mav_put_uint8_t(buf, 252, direction); - _mav_put_uint8_t(buf, 253, flags); - _mav_put_char_array(buf, 12, dest_path, 240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#else - mavlink_file_transfer_start_t packet; - packet.transfer_uid = transfer_uid; - packet.file_size = file_size; - packet.direction = direction; - packet.flags = flags; - mav_array_memcpy(packet.dest_path, dest_path, sizeof(char)*240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_START; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif -} - -/** - * @brief Pack a file_transfer_start message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param transfer_uid Unique transfer ID - * @param dest_path Destination path - * @param direction Transfer direction: 0: from requester, 1: to requester - * @param file_size File size in bytes - * @param flags RESERVED - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_file_transfer_start_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t transfer_uid,const char *dest_path,uint8_t direction,uint32_t file_size,uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint32_t(buf, 8, file_size); - _mav_put_uint8_t(buf, 252, direction); - _mav_put_uint8_t(buf, 253, flags); - _mav_put_char_array(buf, 12, dest_path, 240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#else - mavlink_file_transfer_start_t packet; - packet.transfer_uid = transfer_uid; - packet.file_size = file_size; - packet.direction = direction; - packet.flags = flags; - mav_array_memcpy(packet.dest_path, dest_path, sizeof(char)*240); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_START; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif -} - -/** - * @brief Encode a file_transfer_start struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param file_transfer_start C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_file_transfer_start_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_file_transfer_start_t* file_transfer_start) -{ - return mavlink_msg_file_transfer_start_pack(system_id, component_id, msg, file_transfer_start->transfer_uid, file_transfer_start->dest_path, file_transfer_start->direction, file_transfer_start->file_size, file_transfer_start->flags); -} - -/** - * @brief Encode a file_transfer_start struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param file_transfer_start C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_file_transfer_start_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_start_t* file_transfer_start) -{ - return mavlink_msg_file_transfer_start_pack_chan(system_id, component_id, chan, msg, file_transfer_start->transfer_uid, file_transfer_start->dest_path, file_transfer_start->direction, file_transfer_start->file_size, file_transfer_start->flags); -} - -/** - * @brief Send a file_transfer_start message - * @param chan MAVLink channel to send the message - * - * @param transfer_uid Unique transfer ID - * @param dest_path Destination path - * @param direction Transfer direction: 0: from requester, 1: to requester - * @param file_size File size in bytes - * @param flags RESERVED - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_file_transfer_start_send(mavlink_channel_t chan, uint64_t transfer_uid, const char *dest_path, uint8_t direction, uint32_t file_size, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN]; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint32_t(buf, 8, file_size); - _mav_put_uint8_t(buf, 252, direction); - _mav_put_uint8_t(buf, 253, flags); - _mav_put_char_array(buf, 12, dest_path, 240); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif -#else - mavlink_file_transfer_start_t packet; - packet.transfer_uid = transfer_uid; - packet.file_size = file_size; - packet.direction = direction; - packet.flags = flags; - mav_array_memcpy(packet.dest_path, dest_path, sizeof(char)*240); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_file_transfer_start_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t transfer_uid, const char *dest_path, uint8_t direction, uint32_t file_size, uint8_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, transfer_uid); - _mav_put_uint32_t(buf, 8, file_size); - _mav_put_uint8_t(buf, 252, direction); - _mav_put_uint8_t(buf, 253, flags); - _mav_put_char_array(buf, 12, dest_path, 240); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif -#else - mavlink_file_transfer_start_t *packet = (mavlink_file_transfer_start_t *)msgbuf; - packet->transfer_uid = transfer_uid; - packet->file_size = file_size; - packet->direction = direction; - packet->flags = flags; - mav_array_memcpy(packet->dest_path, dest_path, sizeof(char)*240); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FILE_TRANSFER_START UNPACKING - - -/** - * @brief Get field transfer_uid from file_transfer_start message - * - * @return Unique transfer ID - */ -static inline uint64_t mavlink_msg_file_transfer_start_get_transfer_uid(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field dest_path from file_transfer_start message - * - * @return Destination path - */ -static inline uint16_t mavlink_msg_file_transfer_start_get_dest_path(const mavlink_message_t* msg, char *dest_path) -{ - return _MAV_RETURN_char_array(msg, dest_path, 240, 12); -} - -/** - * @brief Get field direction from file_transfer_start message - * - * @return Transfer direction: 0: from requester, 1: to requester - */ -static inline uint8_t mavlink_msg_file_transfer_start_get_direction(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 252); -} - -/** - * @brief Get field file_size from file_transfer_start message - * - * @return File size in bytes - */ -static inline uint32_t mavlink_msg_file_transfer_start_get_file_size(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 8); -} - -/** - * @brief Get field flags from file_transfer_start message - * - * @return RESERVED - */ -static inline uint8_t mavlink_msg_file_transfer_start_get_flags(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 253); -} - -/** - * @brief Decode a file_transfer_start message into a struct - * - * @param msg The message to decode - * @param file_transfer_start C-struct to decode the message contents into - */ -static inline void mavlink_msg_file_transfer_start_decode(const mavlink_message_t* msg, mavlink_file_transfer_start_t* file_transfer_start) -{ -#if MAVLINK_NEED_BYTE_SWAP - file_transfer_start->transfer_uid = mavlink_msg_file_transfer_start_get_transfer_uid(msg); - file_transfer_start->file_size = mavlink_msg_file_transfer_start_get_file_size(msg); - mavlink_msg_file_transfer_start_get_dest_path(msg, file_transfer_start->dest_path); - file_transfer_start->direction = mavlink_msg_file_transfer_start_get_direction(msg); - file_transfer_start->flags = mavlink_msg_file_transfer_start_get_flags(msg); -#else - memcpy(file_transfer_start, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h deleted file mode 100644 index ba15bf365..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h +++ /dev/null @@ -1,401 +0,0 @@ -// MESSAGE GLOBAL_POSITION_INT PACKING - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 33 - -typedef struct __mavlink_global_position_int_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - int32_t lat; ///< Latitude, expressed as * 1E7 - int32_t lon; ///< Longitude, expressed as * 1E7 - int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters), above MSL - int32_t relative_alt; ///< Altitude above ground in meters, expressed as * 1000 (millimeters) - int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 - int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 - int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 - uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX -} mavlink_global_position_int_t; - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 28 -#define MAVLINK_MSG_ID_33_LEN 28 - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC 104 -#define MAVLINK_MSG_ID_33_CRC 104 - - - -#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT { \ - "GLOBAL_POSITION_INT", \ - 9, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_global_position_int_t, time_boot_ms) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_int_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_int_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_global_position_int_t, alt) }, \ - { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_global_position_int_t, relative_alt) }, \ - { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_global_position_int_t, vx) }, \ - { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_global_position_int_t, vy) }, \ - { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_global_position_int_t, vz) }, \ - { "hdg", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_global_position_int_t, hdg) }, \ - } \ -} - - -/** - * @brief Pack a global_position_int message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL - * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) - * @param vx Ground X Speed (Latitude), expressed as m/s * 100 - * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, lon); - _mav_put_int32_t(buf, 12, alt); - _mav_put_int32_t(buf, 16, relative_alt); - _mav_put_int16_t(buf, 20, vx); - _mav_put_int16_t(buf, 22, vy); - _mav_put_int16_t(buf, 24, vz); - _mav_put_uint16_t(buf, 26, hdg); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#else - mavlink_global_position_int_t packet; - packet.time_boot_ms = time_boot_ms; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.relative_alt = relative_alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.hdg = hdg; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif -} - -/** - * @brief Pack a global_position_int message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL - * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) - * @param vx Ground X Speed (Latitude), expressed as m/s * 100 - * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,int32_t lat,int32_t lon,int32_t alt,int32_t relative_alt,int16_t vx,int16_t vy,int16_t vz,uint16_t hdg) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, lon); - _mav_put_int32_t(buf, 12, alt); - _mav_put_int32_t(buf, 16, relative_alt); - _mav_put_int16_t(buf, 20, vx); - _mav_put_int16_t(buf, 22, vy); - _mav_put_int16_t(buf, 24, vz); - _mav_put_uint16_t(buf, 26, hdg); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#else - mavlink_global_position_int_t packet; - packet.time_boot_ms = time_boot_ms; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.relative_alt = relative_alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.hdg = hdg; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif -} - -/** - * @brief Encode a global_position_int struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param global_position_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) -{ - return mavlink_msg_global_position_int_pack(system_id, component_id, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); -} - -/** - * @brief Encode a global_position_int struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param global_position_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_position_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) -{ - return mavlink_msg_global_position_int_pack_chan(system_id, component_id, chan, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); -} - -/** - * @brief Send a global_position_int message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL - * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) - * @param vx Ground X Speed (Latitude), expressed as m/s * 100 - * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, lon); - _mav_put_int32_t(buf, 12, alt); - _mav_put_int32_t(buf, 16, relative_alt); - _mav_put_int16_t(buf, 20, vx); - _mav_put_int16_t(buf, 22, vy); - _mav_put_int16_t(buf, 24, vz); - _mav_put_uint16_t(buf, 26, hdg); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif -#else - mavlink_global_position_int_t packet; - packet.time_boot_ms = time_boot_ms; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.relative_alt = relative_alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.hdg = hdg; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_global_position_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, lon); - _mav_put_int32_t(buf, 12, alt); - _mav_put_int32_t(buf, 16, relative_alt); - _mav_put_int16_t(buf, 20, vx); - _mav_put_int16_t(buf, 22, vy); - _mav_put_int16_t(buf, 24, vz); - _mav_put_uint16_t(buf, 26, hdg); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif -#else - mavlink_global_position_int_t *packet = (mavlink_global_position_int_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->lat = lat; - packet->lon = lon; - packet->alt = alt; - packet->relative_alt = relative_alt; - packet->vx = vx; - packet->vy = vy; - packet->vz = vz; - packet->hdg = hdg; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE GLOBAL_POSITION_INT UNPACKING - - -/** - * @brief Get field time_boot_ms from global_position_int message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_global_position_int_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field lat from global_position_int message - * - * @return Latitude, expressed as * 1E7 - */ -static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field lon from global_position_int message - * - * @return Longitude, expressed as * 1E7 - */ -static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field alt from global_position_int message - * - * @return Altitude in meters, expressed as * 1000 (millimeters), above MSL - */ -static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 12); -} - -/** - * @brief Get field relative_alt from global_position_int message - * - * @return Altitude above ground in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_global_position_int_get_relative_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 16); -} - -/** - * @brief Get field vx from global_position_int message - * - * @return Ground X Speed (Latitude), expressed as m/s * 100 - */ -static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Get field vy from global_position_int message - * - * @return Ground Y Speed (Longitude), expressed as m/s * 100 - */ -static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 22); -} - -/** - * @brief Get field vz from global_position_int message - * - * @return Ground Z Speed (Altitude), expressed as m/s * 100 - */ -static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 24); -} - -/** - * @brief Get field hdg from global_position_int message - * - * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - */ -static inline uint16_t mavlink_msg_global_position_int_get_hdg(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 26); -} - -/** - * @brief Decode a global_position_int message into a struct - * - * @param msg The message to decode - * @param global_position_int C-struct to decode the message contents into - */ -static inline void mavlink_msg_global_position_int_decode(const mavlink_message_t* msg, mavlink_global_position_int_t* global_position_int) -{ -#if MAVLINK_NEED_BYTE_SWAP - global_position_int->time_boot_ms = mavlink_msg_global_position_int_get_time_boot_ms(msg); - global_position_int->lat = mavlink_msg_global_position_int_get_lat(msg); - global_position_int->lon = mavlink_msg_global_position_int_get_lon(msg); - global_position_int->alt = mavlink_msg_global_position_int_get_alt(msg); - global_position_int->relative_alt = mavlink_msg_global_position_int_get_relative_alt(msg); - global_position_int->vx = mavlink_msg_global_position_int_get_vx(msg); - global_position_int->vy = mavlink_msg_global_position_int_get_vy(msg); - global_position_int->vz = mavlink_msg_global_position_int_get_vz(msg); - global_position_int->hdg = mavlink_msg_global_position_int_get_hdg(msg); -#else - memcpy(global_position_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h deleted file mode 100644 index 77d735cf4..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE GLOBAL_POSITION_SETPOINT_INT PACKING - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT 52 - -typedef struct __mavlink_global_position_setpoint_int_t -{ - int32_t latitude; ///< Latitude (WGS84), in degrees * 1E7 - int32_t longitude; ///< Longitude (WGS84), in degrees * 1E7 - int32_t altitude; ///< Altitude (WGS84), in meters * 1000 (positive for up) - int16_t yaw; ///< Desired yaw angle in degrees * 100 - uint8_t coordinate_frame; ///< Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT -} mavlink_global_position_setpoint_int_t; - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN 15 -#define MAVLINK_MSG_ID_52_LEN 15 - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC 141 -#define MAVLINK_MSG_ID_52_CRC 141 - - - -#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT { \ - "GLOBAL_POSITION_SETPOINT_INT", \ - 5, \ - { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_global_position_setpoint_int_t, latitude) }, \ - { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_setpoint_int_t, longitude) }, \ - { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_setpoint_int_t, altitude) }, \ - { "yaw", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_global_position_setpoint_int_t, yaw) }, \ - { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_global_position_setpoint_int_t, coordinate_frame) }, \ - } \ -} - - -/** - * @brief Pack a global_position_setpoint_int message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_position_setpoint_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#else - mavlink_global_position_setpoint_int_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -} - -/** - * @brief Pack a global_position_setpoint_int message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_position_setpoint_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t coordinate_frame,int32_t latitude,int32_t longitude,int32_t altitude,int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#else - mavlink_global_position_setpoint_int_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -} - -/** - * @brief Encode a global_position_setpoint_int struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param global_position_setpoint_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_position_setpoint_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_setpoint_int_t* global_position_setpoint_int) -{ - return mavlink_msg_global_position_setpoint_int_pack(system_id, component_id, msg, global_position_setpoint_int->coordinate_frame, global_position_setpoint_int->latitude, global_position_setpoint_int->longitude, global_position_setpoint_int->altitude, global_position_setpoint_int->yaw); -} - -/** - * @brief Encode a global_position_setpoint_int struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param global_position_setpoint_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_position_setpoint_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_setpoint_int_t* global_position_setpoint_int) -{ - return mavlink_msg_global_position_setpoint_int_pack_chan(system_id, component_id, chan, msg, global_position_setpoint_int->coordinate_frame, global_position_setpoint_int->latitude, global_position_setpoint_int->longitude, global_position_setpoint_int->altitude, global_position_setpoint_int->yaw); -} - -/** - * @brief Send a global_position_setpoint_int message - * @param chan MAVLink channel to send the message - * - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_global_position_setpoint_int_send(mavlink_channel_t chan, uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -#else - mavlink_global_position_setpoint_int_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_global_position_setpoint_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -#else - mavlink_global_position_setpoint_int_t *packet = (mavlink_global_position_setpoint_int_t *)msgbuf; - packet->latitude = latitude; - packet->longitude = longitude; - packet->altitude = altitude; - packet->yaw = yaw; - packet->coordinate_frame = coordinate_frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE GLOBAL_POSITION_SETPOINT_INT UNPACKING - - -/** - * @brief Get field coordinate_frame from global_position_setpoint_int message - * - * @return Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - */ -static inline uint8_t mavlink_msg_global_position_setpoint_int_get_coordinate_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field latitude from global_position_setpoint_int message - * - * @return Latitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_global_position_setpoint_int_get_latitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field longitude from global_position_setpoint_int message - * - * @return Longitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_global_position_setpoint_int_get_longitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field altitude from global_position_setpoint_int message - * - * @return Altitude (WGS84), in meters * 1000 (positive for up) - */ -static inline int32_t mavlink_msg_global_position_setpoint_int_get_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field yaw from global_position_setpoint_int message - * - * @return Desired yaw angle in degrees * 100 - */ -static inline int16_t mavlink_msg_global_position_setpoint_int_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Decode a global_position_setpoint_int message into a struct - * - * @param msg The message to decode - * @param global_position_setpoint_int C-struct to decode the message contents into - */ -static inline void mavlink_msg_global_position_setpoint_int_decode(const mavlink_message_t* msg, mavlink_global_position_setpoint_int_t* global_position_setpoint_int) -{ -#if MAVLINK_NEED_BYTE_SWAP - global_position_setpoint_int->latitude = mavlink_msg_global_position_setpoint_int_get_latitude(msg); - global_position_setpoint_int->longitude = mavlink_msg_global_position_setpoint_int_get_longitude(msg); - global_position_setpoint_int->altitude = mavlink_msg_global_position_setpoint_int_get_altitude(msg); - global_position_setpoint_int->yaw = mavlink_msg_global_position_setpoint_int_get_yaw(msg); - global_position_setpoint_int->coordinate_frame = mavlink_msg_global_position_setpoint_int_get_coordinate_frame(msg); -#else - memcpy(global_position_setpoint_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h deleted file mode 100644 index b2b75d7ef..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE GLOBAL_VISION_POSITION_ESTIMATE PACKING - -#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE 101 - -typedef struct __mavlink_global_vision_position_estimate_t -{ - uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) - float x; ///< Global X position - float y; ///< Global Y position - float z; ///< Global Z position - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad -} mavlink_global_vision_position_estimate_t; - -#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN 32 -#define MAVLINK_MSG_ID_101_LEN 32 - -#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC 102 -#define MAVLINK_MSG_ID_101_CRC 102 - - - -#define MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE { \ - "GLOBAL_VISION_POSITION_ESTIMATE", \ - 7, \ - { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_vision_position_estimate_t, usec) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_global_vision_position_estimate_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_vision_position_estimate_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_global_vision_position_estimate_t, z) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_vision_position_estimate_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_vision_position_estimate_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_vision_position_estimate_t, yaw) }, \ - } \ -} - - -/** - * @brief Pack a global_vision_position_estimate message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, roll); - _mav_put_float(buf, 24, pitch); - _mav_put_float(buf, 28, yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); -#else - mavlink_global_vision_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); -#endif -} - -/** - * @brief Pack a global_vision_position_estimate message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, roll); - _mav_put_float(buf, 24, pitch); - _mav_put_float(buf, 28, yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); -#else - mavlink_global_vision_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); -#endif -} - -/** - * @brief Encode a global_vision_position_estimate struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param global_vision_position_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) -{ - return mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); -} - -/** - * @brief Encode a global_vision_position_estimate struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param global_vision_position_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) -{ - return mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); -} - -/** - * @brief Send a global_vision_position_estimate message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, roll); - _mav_put_float(buf, 24, pitch); - _mav_put_float(buf, 28, yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); -#endif -#else - mavlink_global_vision_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, roll); - _mav_put_float(buf, 24, pitch); - _mav_put_float(buf, 28, yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); -#endif -#else - mavlink_global_vision_position_estimate_t *packet = (mavlink_global_vision_position_estimate_t *)msgbuf; - packet->usec = usec; - packet->x = x; - packet->y = y; - packet->z = z; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE GLOBAL_VISION_POSITION_ESTIMATE UNPACKING - - -/** - * @brief Get field usec from global_vision_position_estimate message - * - * @return Timestamp (microseconds, synced to UNIX time or since system boot) - */ -static inline uint64_t mavlink_msg_global_vision_position_estimate_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field x from global_vision_position_estimate message - * - * @return Global X position - */ -static inline float mavlink_msg_global_vision_position_estimate_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field y from global_vision_position_estimate message - * - * @return Global Y position - */ -static inline float mavlink_msg_global_vision_position_estimate_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field z from global_vision_position_estimate message - * - * @return Global Z position - */ -static inline float mavlink_msg_global_vision_position_estimate_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field roll from global_vision_position_estimate message - * - * @return Roll angle in rad - */ -static inline float mavlink_msg_global_vision_position_estimate_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field pitch from global_vision_position_estimate message - * - * @return Pitch angle in rad - */ -static inline float mavlink_msg_global_vision_position_estimate_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field yaw from global_vision_position_estimate message - * - * @return Yaw angle in rad - */ -static inline float mavlink_msg_global_vision_position_estimate_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a global_vision_position_estimate message into a struct - * - * @param msg The message to decode - * @param global_vision_position_estimate C-struct to decode the message contents into - */ -static inline void mavlink_msg_global_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_global_vision_position_estimate_t* global_vision_position_estimate) -{ -#if MAVLINK_NEED_BYTE_SWAP - global_vision_position_estimate->usec = mavlink_msg_global_vision_position_estimate_get_usec(msg); - global_vision_position_estimate->x = mavlink_msg_global_vision_position_estimate_get_x(msg); - global_vision_position_estimate->y = mavlink_msg_global_vision_position_estimate_get_y(msg); - global_vision_position_estimate->z = mavlink_msg_global_vision_position_estimate_get_z(msg); - global_vision_position_estimate->roll = mavlink_msg_global_vision_position_estimate_get_roll(msg); - global_vision_position_estimate->pitch = mavlink_msg_global_vision_position_estimate_get_pitch(msg); - global_vision_position_estimate->yaw = mavlink_msg_global_vision_position_estimate_get_yaw(msg); -#else - memcpy(global_vision_position_estimate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h deleted file mode 100644 index b184e7c9c..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h +++ /dev/null @@ -1,473 +0,0 @@ -// MESSAGE GPS2_RAW PACKING - -#define MAVLINK_MSG_ID_GPS2_RAW 124 - -typedef struct __mavlink_gps2_raw_t -{ - uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int32_t lat; ///< Latitude (WGS84), in degrees * 1E7 - int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 - int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up) - uint32_t dgps_age; ///< Age of DGPS info - uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX - uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 - uint8_t dgps_numch; ///< Number of DGPS satellites -} mavlink_gps2_raw_t; - -#define MAVLINK_MSG_ID_GPS2_RAW_LEN 35 -#define MAVLINK_MSG_ID_124_LEN 35 - -#define MAVLINK_MSG_ID_GPS2_RAW_CRC 87 -#define MAVLINK_MSG_ID_124_CRC 87 - - - -#define MAVLINK_MESSAGE_INFO_GPS2_RAW { \ - "GPS2_RAW", \ - 12, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps2_raw_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps2_raw_t, alt) }, \ - { "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \ - { "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps2_raw_t, eph) }, \ - { "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps2_raw_t, epv) }, \ - { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps2_raw_t, vel) }, \ - { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_gps2_raw_t, cog) }, \ - { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \ - { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_raw_t, satellites_visible) }, \ - { "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \ - } \ -} - - -/** - * @brief Pack a gps2_raw message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude (WGS84), in degrees * 1E7 - * @param lon Longitude (WGS84), in degrees * 1E7 - * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @param satellites_visible Number of satellites visible. If unknown, set to 255 - * @param dgps_numch Number of DGPS satellites - * @param dgps_age Age of DGPS info - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int32_t(buf, 8, lat); - _mav_put_int32_t(buf, 12, lon); - _mav_put_int32_t(buf, 16, alt); - _mav_put_uint32_t(buf, 20, dgps_age); - _mav_put_uint16_t(buf, 24, eph); - _mav_put_uint16_t(buf, 26, epv); - _mav_put_uint16_t(buf, 28, vel); - _mav_put_uint16_t(buf, 30, cog); - _mav_put_uint8_t(buf, 32, fix_type); - _mav_put_uint8_t(buf, 33, satellites_visible); - _mav_put_uint8_t(buf, 34, dgps_numch); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); -#else - mavlink_gps2_raw_t packet; - packet.time_usec = time_usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.dgps_age = dgps_age; - packet.eph = eph; - packet.epv = epv; - packet.vel = vel; - packet.cog = cog; - packet.fix_type = fix_type; - packet.satellites_visible = satellites_visible; - packet.dgps_numch = dgps_numch; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS2_RAW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN); -#endif -} - -/** - * @brief Pack a gps2_raw message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude (WGS84), in degrees * 1E7 - * @param lon Longitude (WGS84), in degrees * 1E7 - * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @param satellites_visible Number of satellites visible. If unknown, set to 255 - * @param dgps_numch Number of DGPS satellites - * @param dgps_age Age of DGPS info - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,uint8_t dgps_numch,uint32_t dgps_age) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int32_t(buf, 8, lat); - _mav_put_int32_t(buf, 12, lon); - _mav_put_int32_t(buf, 16, alt); - _mav_put_uint32_t(buf, 20, dgps_age); - _mav_put_uint16_t(buf, 24, eph); - _mav_put_uint16_t(buf, 26, epv); - _mav_put_uint16_t(buf, 28, vel); - _mav_put_uint16_t(buf, 30, cog); - _mav_put_uint8_t(buf, 32, fix_type); - _mav_put_uint8_t(buf, 33, satellites_visible); - _mav_put_uint8_t(buf, 34, dgps_numch); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); -#else - mavlink_gps2_raw_t packet; - packet.time_usec = time_usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.dgps_age = dgps_age; - packet.eph = eph; - packet.epv = epv; - packet.vel = vel; - packet.cog = cog; - packet.fix_type = fix_type; - packet.satellites_visible = satellites_visible; - packet.dgps_numch = dgps_numch; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS2_RAW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN); -#endif -} - -/** - * @brief Encode a gps2_raw struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param gps2_raw C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) -{ - return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); -} - -/** - * @brief Encode a gps2_raw struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param gps2_raw C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) -{ - return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); -} - -/** - * @brief Send a gps2_raw message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude (WGS84), in degrees * 1E7 - * @param lon Longitude (WGS84), in degrees * 1E7 - * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @param satellites_visible Number of satellites visible. If unknown, set to 255 - * @param dgps_numch Number of DGPS satellites - * @param dgps_age Age of DGPS info - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int32_t(buf, 8, lat); - _mav_put_int32_t(buf, 12, lon); - _mav_put_int32_t(buf, 16, alt); - _mav_put_uint32_t(buf, 20, dgps_age); - _mav_put_uint16_t(buf, 24, eph); - _mav_put_uint16_t(buf, 26, epv); - _mav_put_uint16_t(buf, 28, vel); - _mav_put_uint16_t(buf, 30, cog); - _mav_put_uint8_t(buf, 32, fix_type); - _mav_put_uint8_t(buf, 33, satellites_visible); - _mav_put_uint8_t(buf, 34, dgps_numch); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); -#endif -#else - mavlink_gps2_raw_t packet; - packet.time_usec = time_usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.dgps_age = dgps_age; - packet.eph = eph; - packet.epv = epv; - packet.vel = vel; - packet.cog = cog; - packet.fix_type = fix_type; - packet.satellites_visible = satellites_visible; - packet.dgps_numch = dgps_numch; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_GPS2_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int32_t(buf, 8, lat); - _mav_put_int32_t(buf, 12, lon); - _mav_put_int32_t(buf, 16, alt); - _mav_put_uint32_t(buf, 20, dgps_age); - _mav_put_uint16_t(buf, 24, eph); - _mav_put_uint16_t(buf, 26, epv); - _mav_put_uint16_t(buf, 28, vel); - _mav_put_uint16_t(buf, 30, cog); - _mav_put_uint8_t(buf, 32, fix_type); - _mav_put_uint8_t(buf, 33, satellites_visible); - _mav_put_uint8_t(buf, 34, dgps_numch); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); -#endif -#else - mavlink_gps2_raw_t *packet = (mavlink_gps2_raw_t *)msgbuf; - packet->time_usec = time_usec; - packet->lat = lat; - packet->lon = lon; - packet->alt = alt; - packet->dgps_age = dgps_age; - packet->eph = eph; - packet->epv = epv; - packet->vel = vel; - packet->cog = cog; - packet->fix_type = fix_type; - packet->satellites_visible = satellites_visible; - packet->dgps_numch = dgps_numch; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE GPS2_RAW UNPACKING - - -/** - * @brief Get field time_usec from gps2_raw message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_gps2_raw_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field fix_type from gps2_raw message - * - * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - */ -static inline uint8_t mavlink_msg_gps2_raw_get_fix_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field lat from gps2_raw message - * - * @return Latitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_gps2_raw_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field lon from gps2_raw message - * - * @return Longitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_gps2_raw_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 12); -} - -/** - * @brief Get field alt from gps2_raw message - * - * @return Altitude (WGS84), in meters * 1000 (positive for up) - */ -static inline int32_t mavlink_msg_gps2_raw_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 16); -} - -/** - * @brief Get field eph from gps2_raw message - * - * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - */ -static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 24); -} - -/** - * @brief Get field epv from gps2_raw message - * - * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - */ -static inline uint16_t mavlink_msg_gps2_raw_get_epv(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 26); -} - -/** - * @brief Get field vel from gps2_raw message - * - * @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX - */ -static inline uint16_t mavlink_msg_gps2_raw_get_vel(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 28); -} - -/** - * @brief Get field cog from gps2_raw message - * - * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - */ -static inline uint16_t mavlink_msg_gps2_raw_get_cog(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 30); -} - -/** - * @brief Get field satellites_visible from gps2_raw message - * - * @return Number of satellites visible. If unknown, set to 255 - */ -static inline uint8_t mavlink_msg_gps2_raw_get_satellites_visible(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 33); -} - -/** - * @brief Get field dgps_numch from gps2_raw message - * - * @return Number of DGPS satellites - */ -static inline uint8_t mavlink_msg_gps2_raw_get_dgps_numch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 34); -} - -/** - * @brief Get field dgps_age from gps2_raw message - * - * @return Age of DGPS info - */ -static inline uint32_t mavlink_msg_gps2_raw_get_dgps_age(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 20); -} - -/** - * @brief Decode a gps2_raw message into a struct - * - * @param msg The message to decode - * @param gps2_raw C-struct to decode the message contents into - */ -static inline void mavlink_msg_gps2_raw_decode(const mavlink_message_t* msg, mavlink_gps2_raw_t* gps2_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP - gps2_raw->time_usec = mavlink_msg_gps2_raw_get_time_usec(msg); - gps2_raw->lat = mavlink_msg_gps2_raw_get_lat(msg); - gps2_raw->lon = mavlink_msg_gps2_raw_get_lon(msg); - gps2_raw->alt = mavlink_msg_gps2_raw_get_alt(msg); - gps2_raw->dgps_age = mavlink_msg_gps2_raw_get_dgps_age(msg); - gps2_raw->eph = mavlink_msg_gps2_raw_get_eph(msg); - gps2_raw->epv = mavlink_msg_gps2_raw_get_epv(msg); - gps2_raw->vel = mavlink_msg_gps2_raw_get_vel(msg); - gps2_raw->cog = mavlink_msg_gps2_raw_get_cog(msg); - gps2_raw->fix_type = mavlink_msg_gps2_raw_get_fix_type(msg); - gps2_raw->satellites_visible = mavlink_msg_gps2_raw_get_satellites_visible(msg); - gps2_raw->dgps_numch = mavlink_msg_gps2_raw_get_dgps_numch(msg); -#else - memcpy(gps2_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS2_RAW_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h deleted file mode 100644 index 1589c8ca5..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE GPS_GLOBAL_ORIGIN PACKING - -#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN 49 - -typedef struct __mavlink_gps_global_origin_t -{ - int32_t latitude; ///< Latitude (WGS84), in degrees * 1E7 - int32_t longitude; ///< Longitude (WGS84), in degrees * 1E7 - int32_t altitude; ///< Altitude (WGS84), in meters * 1000 (positive for up) -} mavlink_gps_global_origin_t; - -#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN 12 -#define MAVLINK_MSG_ID_49_LEN 12 - -#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC 39 -#define MAVLINK_MSG_ID_49_CRC 39 - - - -#define MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN { \ - "GPS_GLOBAL_ORIGIN", \ - 3, \ - { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_global_origin_t, latitude) }, \ - { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_global_origin_t, longitude) }, \ - { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_global_origin_t, altitude) }, \ - } \ -} - - -/** - * @brief Pack a gps_global_origin message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int32_t latitude, int32_t longitude, int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); -#else - mavlink_gps_global_origin_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); -#endif -} - -/** - * @brief Pack a gps_global_origin message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - int32_t latitude,int32_t longitude,int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); -#else - mavlink_gps_global_origin_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); -#endif -} - -/** - * @brief Encode a gps_global_origin struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param gps_global_origin C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin) -{ - return mavlink_msg_gps_global_origin_pack(system_id, component_id, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); -} - -/** - * @brief Encode a gps_global_origin struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param gps_global_origin C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin) -{ - return mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, chan, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); -} - -/** - * @brief Send a gps_global_origin message - * @param chan MAVLink channel to send the message - * - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); -#endif -#else - mavlink_gps_global_origin_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)&packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)&packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); -#endif -#else - mavlink_gps_global_origin_t *packet = (mavlink_gps_global_origin_t *)msgbuf; - packet->latitude = latitude; - packet->longitude = longitude; - packet->altitude = altitude; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE GPS_GLOBAL_ORIGIN UNPACKING - - -/** - * @brief Get field latitude from gps_global_origin message - * - * @return Latitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_gps_global_origin_get_latitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field longitude from gps_global_origin message - * - * @return Longitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_gps_global_origin_get_longitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field altitude from gps_global_origin message - * - * @return Altitude (WGS84), in meters * 1000 (positive for up) - */ -static inline int32_t mavlink_msg_gps_global_origin_get_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Decode a gps_global_origin message into a struct - * - * @param msg The message to decode - * @param gps_global_origin C-struct to decode the message contents into - */ -static inline void mavlink_msg_gps_global_origin_decode(const mavlink_message_t* msg, mavlink_gps_global_origin_t* gps_global_origin) -{ -#if MAVLINK_NEED_BYTE_SWAP - gps_global_origin->latitude = mavlink_msg_gps_global_origin_get_latitude(msg); - gps_global_origin->longitude = mavlink_msg_gps_global_origin_get_longitude(msg); - gps_global_origin->altitude = mavlink_msg_gps_global_origin_get_altitude(msg); -#else - memcpy(gps_global_origin, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h deleted file mode 100644 index 362e2d7b9..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h +++ /dev/null @@ -1,273 +0,0 @@ -// MESSAGE GPS_INJECT_DATA PACKING - -#define MAVLINK_MSG_ID_GPS_INJECT_DATA 123 - -typedef struct __mavlink_gps_inject_data_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t len; ///< data length - uint8_t data[110]; ///< raw data (110 is enough for 12 satellites of RTCMv2) -} mavlink_gps_inject_data_t; - -#define MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN 113 -#define MAVLINK_MSG_ID_123_LEN 113 - -#define MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC 250 -#define MAVLINK_MSG_ID_123_CRC 250 - -#define MAVLINK_MSG_GPS_INJECT_DATA_FIELD_DATA_LEN 110 - -#define MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA { \ - "GPS_INJECT_DATA", \ - 4, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_inject_data_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gps_inject_data_t, target_component) }, \ - { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_gps_inject_data_t, len) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 110, 3, offsetof(mavlink_gps_inject_data_t, data) }, \ - } \ -} - - -/** - * @brief Pack a gps_inject_data message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param len data length - * @param data raw data (110 is enough for 12 satellites of RTCMv2) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_inject_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, len); - _mav_put_uint8_t_array(buf, 3, data, 110); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); -#else - mavlink_gps_inject_data_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); -#endif -} - -/** - * @brief Pack a gps_inject_data message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param len data length - * @param data raw data (110 is enough for 12 satellites of RTCMv2) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_inject_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t len,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, len); - _mav_put_uint8_t_array(buf, 3, data, 110); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); -#else - mavlink_gps_inject_data_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); -#endif -} - -/** - * @brief Encode a gps_inject_data struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param gps_inject_data C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_inject_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data) -{ - return mavlink_msg_gps_inject_data_pack(system_id, component_id, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data); -} - -/** - * @brief Encode a gps_inject_data struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param gps_inject_data C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_inject_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data) -{ - return mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, chan, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data); -} - -/** - * @brief Send a gps_inject_data message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param len data length - * @param data raw data (110 is enough for 12 satellites of RTCMv2) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps_inject_data_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, len); - _mav_put_uint8_t_array(buf, 3, data, 110); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); -#endif -#else - mavlink_gps_inject_data_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_gps_inject_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, len); - _mav_put_uint8_t_array(buf, 3, data, 110); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); -#endif -#else - mavlink_gps_inject_data_t *packet = (mavlink_gps_inject_data_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - packet->len = len; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*110); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE GPS_INJECT_DATA UNPACKING - - -/** - * @brief Get field target_system from gps_inject_data message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_gps_inject_data_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from gps_inject_data message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_gps_inject_data_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field len from gps_inject_data message - * - * @return data length - */ -static inline uint8_t mavlink_msg_gps_inject_data_get_len(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field data from gps_inject_data message - * - * @return raw data (110 is enough for 12 satellites of RTCMv2) - */ -static inline uint16_t mavlink_msg_gps_inject_data_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 110, 3); -} - -/** - * @brief Decode a gps_inject_data message into a struct - * - * @param msg The message to decode - * @param gps_inject_data C-struct to decode the message contents into - */ -static inline void mavlink_msg_gps_inject_data_decode(const mavlink_message_t* msg, mavlink_gps_inject_data_t* gps_inject_data) -{ -#if MAVLINK_NEED_BYTE_SWAP - gps_inject_data->target_system = mavlink_msg_gps_inject_data_get_target_system(msg); - gps_inject_data->target_component = mavlink_msg_gps_inject_data_get_target_component(msg); - gps_inject_data->len = mavlink_msg_gps_inject_data_get_len(msg); - mavlink_msg_gps_inject_data_get_data(msg, gps_inject_data->data); -#else - memcpy(gps_inject_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h deleted file mode 100644 index b53996206..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h +++ /dev/null @@ -1,425 +0,0 @@ -// MESSAGE GPS_RAW_INT PACKING - -#define MAVLINK_MSG_ID_GPS_RAW_INT 24 - -typedef struct __mavlink_gps_raw_int_t -{ - uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int32_t lat; ///< Latitude (WGS84), in degrees * 1E7 - int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 - int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up) - uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX - uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 -} mavlink_gps_raw_int_t; - -#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 30 -#define MAVLINK_MSG_ID_24_LEN 30 - -#define MAVLINK_MSG_ID_GPS_RAW_INT_CRC 24 -#define MAVLINK_MSG_ID_24_CRC 24 - - - -#define MAVLINK_MESSAGE_INFO_GPS_RAW_INT { \ - "GPS_RAW_INT", \ - 10, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_int_t, time_usec) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_raw_int_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps_raw_int_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps_raw_int_t, alt) }, \ - { "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_gps_raw_int_t, eph) }, \ - { "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_gps_raw_int_t, epv) }, \ - { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps_raw_int_t, vel) }, \ - { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps_raw_int_t, cog) }, \ - { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_gps_raw_int_t, fix_type) }, \ - { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_gps_raw_int_t, satellites_visible) }, \ - } \ -} - - -/** - * @brief Pack a gps_raw_int message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude (WGS84), in degrees * 1E7 - * @param lon Longitude (WGS84), in degrees * 1E7 - * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @param satellites_visible Number of satellites visible. If unknown, set to 255 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int32_t(buf, 8, lat); - _mav_put_int32_t(buf, 12, lon); - _mav_put_int32_t(buf, 16, alt); - _mav_put_uint16_t(buf, 20, eph); - _mav_put_uint16_t(buf, 22, epv); - _mav_put_uint16_t(buf, 24, vel); - _mav_put_uint16_t(buf, 26, cog); - _mav_put_uint8_t(buf, 28, fix_type); - _mav_put_uint8_t(buf, 29, satellites_visible); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); -#else - mavlink_gps_raw_int_t packet; - packet.time_usec = time_usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.eph = eph; - packet.epv = epv; - packet.vel = vel; - packet.cog = cog; - packet.fix_type = fix_type; - packet.satellites_visible = satellites_visible; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); -#endif -} - -/** - * @brief Pack a gps_raw_int message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude (WGS84), in degrees * 1E7 - * @param lon Longitude (WGS84), in degrees * 1E7 - * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @param satellites_visible Number of satellites visible. If unknown, set to 255 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int32_t(buf, 8, lat); - _mav_put_int32_t(buf, 12, lon); - _mav_put_int32_t(buf, 16, alt); - _mav_put_uint16_t(buf, 20, eph); - _mav_put_uint16_t(buf, 22, epv); - _mav_put_uint16_t(buf, 24, vel); - _mav_put_uint16_t(buf, 26, cog); - _mav_put_uint8_t(buf, 28, fix_type); - _mav_put_uint8_t(buf, 29, satellites_visible); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); -#else - mavlink_gps_raw_int_t packet; - packet.time_usec = time_usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.eph = eph; - packet.epv = epv; - packet.vel = vel; - packet.cog = cog; - packet.fix_type = fix_type; - packet.satellites_visible = satellites_visible; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); -#endif -} - -/** - * @brief Encode a gps_raw_int struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param gps_raw_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) -{ - return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); -} - -/** - * @brief Encode a gps_raw_int struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param gps_raw_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) -{ - return mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, chan, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); -} - -/** - * @brief Send a gps_raw_int message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude (WGS84), in degrees * 1E7 - * @param lon Longitude (WGS84), in degrees * 1E7 - * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @param satellites_visible Number of satellites visible. If unknown, set to 255 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int32_t(buf, 8, lat); - _mav_put_int32_t(buf, 12, lon); - _mav_put_int32_t(buf, 16, alt); - _mav_put_uint16_t(buf, 20, eph); - _mav_put_uint16_t(buf, 22, epv); - _mav_put_uint16_t(buf, 24, vel); - _mav_put_uint16_t(buf, 26, cog); - _mav_put_uint8_t(buf, 28, fix_type); - _mav_put_uint8_t(buf, 29, satellites_visible); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); -#endif -#else - mavlink_gps_raw_int_t packet; - packet.time_usec = time_usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.eph = eph; - packet.epv = epv; - packet.vel = vel; - packet.cog = cog; - packet.fix_type = fix_type; - packet.satellites_visible = satellites_visible; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)&packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)&packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_GPS_RAW_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int32_t(buf, 8, lat); - _mav_put_int32_t(buf, 12, lon); - _mav_put_int32_t(buf, 16, alt); - _mav_put_uint16_t(buf, 20, eph); - _mav_put_uint16_t(buf, 22, epv); - _mav_put_uint16_t(buf, 24, vel); - _mav_put_uint16_t(buf, 26, cog); - _mav_put_uint8_t(buf, 28, fix_type); - _mav_put_uint8_t(buf, 29, satellites_visible); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); -#endif -#else - mavlink_gps_raw_int_t *packet = (mavlink_gps_raw_int_t *)msgbuf; - packet->time_usec = time_usec; - packet->lat = lat; - packet->lon = lon; - packet->alt = alt; - packet->eph = eph; - packet->epv = epv; - packet->vel = vel; - packet->cog = cog; - packet->fix_type = fix_type; - packet->satellites_visible = satellites_visible; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE GPS_RAW_INT UNPACKING - - -/** - * @brief Get field time_usec from gps_raw_int message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_gps_raw_int_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field fix_type from gps_raw_int message - * - * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - */ -static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 28); -} - -/** - * @brief Get field lat from gps_raw_int message - * - * @return Latitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field lon from gps_raw_int message - * - * @return Longitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 12); -} - -/** - * @brief Get field alt from gps_raw_int message - * - * @return Altitude (WGS84), in meters * 1000 (positive for up) - */ -static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 16); -} - -/** - * @brief Get field eph from gps_raw_int message - * - * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - */ -static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 20); -} - -/** - * @brief Get field epv from gps_raw_int message - * - * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX - */ -static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 22); -} - -/** - * @brief Get field vel from gps_raw_int message - * - * @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX - */ -static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 24); -} - -/** - * @brief Get field cog from gps_raw_int message - * - * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - */ -static inline uint16_t mavlink_msg_gps_raw_int_get_cog(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 26); -} - -/** - * @brief Get field satellites_visible from gps_raw_int message - * - * @return Number of satellites visible. If unknown, set to 255 - */ -static inline uint8_t mavlink_msg_gps_raw_int_get_satellites_visible(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 29); -} - -/** - * @brief Decode a gps_raw_int message into a struct - * - * @param msg The message to decode - * @param gps_raw_int C-struct to decode the message contents into - */ -static inline void mavlink_msg_gps_raw_int_decode(const mavlink_message_t* msg, mavlink_gps_raw_int_t* gps_raw_int) -{ -#if MAVLINK_NEED_BYTE_SWAP - gps_raw_int->time_usec = mavlink_msg_gps_raw_int_get_time_usec(msg); - gps_raw_int->lat = mavlink_msg_gps_raw_int_get_lat(msg); - gps_raw_int->lon = mavlink_msg_gps_raw_int_get_lon(msg); - gps_raw_int->alt = mavlink_msg_gps_raw_int_get_alt(msg); - gps_raw_int->eph = mavlink_msg_gps_raw_int_get_eph(msg); - gps_raw_int->epv = mavlink_msg_gps_raw_int_get_epv(msg); - gps_raw_int->vel = mavlink_msg_gps_raw_int_get_vel(msg); - gps_raw_int->cog = mavlink_msg_gps_raw_int_get_cog(msg); - gps_raw_int->fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); - gps_raw_int->satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(msg); -#else - memcpy(gps_raw_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_RAW_INT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h deleted file mode 100644 index 10659d0dd..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h +++ /dev/null @@ -1,325 +0,0 @@ -// MESSAGE GPS_STATUS PACKING - -#define MAVLINK_MSG_ID_GPS_STATUS 25 - -typedef struct __mavlink_gps_status_t -{ - uint8_t satellites_visible; ///< Number of satellites visible - uint8_t satellite_prn[20]; ///< Global satellite ID - uint8_t satellite_used[20]; ///< 0: Satellite not used, 1: used for localization - uint8_t satellite_elevation[20]; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite - uint8_t satellite_azimuth[20]; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. - uint8_t satellite_snr[20]; ///< Signal to noise ratio of satellite -} mavlink_gps_status_t; - -#define MAVLINK_MSG_ID_GPS_STATUS_LEN 101 -#define MAVLINK_MSG_ID_25_LEN 101 - -#define MAVLINK_MSG_ID_GPS_STATUS_CRC 23 -#define MAVLINK_MSG_ID_25_CRC 23 - -#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_PRN_LEN 20 -#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_USED_LEN 20 -#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_ELEVATION_LEN 20 -#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_AZIMUTH_LEN 20 -#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_SNR_LEN 20 - -#define MAVLINK_MESSAGE_INFO_GPS_STATUS { \ - "GPS_STATUS", \ - 6, \ - { { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_status_t, satellites_visible) }, \ - { "satellite_prn", NULL, MAVLINK_TYPE_UINT8_T, 20, 1, offsetof(mavlink_gps_status_t, satellite_prn) }, \ - { "satellite_used", NULL, MAVLINK_TYPE_UINT8_T, 20, 21, offsetof(mavlink_gps_status_t, satellite_used) }, \ - { "satellite_elevation", NULL, MAVLINK_TYPE_UINT8_T, 20, 41, offsetof(mavlink_gps_status_t, satellite_elevation) }, \ - { "satellite_azimuth", NULL, MAVLINK_TYPE_UINT8_T, 20, 61, offsetof(mavlink_gps_status_t, satellite_azimuth) }, \ - { "satellite_snr", NULL, MAVLINK_TYPE_UINT8_T, 20, 81, offsetof(mavlink_gps_status_t, satellite_snr) }, \ - } \ -} - - -/** - * @brief Pack a gps_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param satellites_visible Number of satellites visible - * @param satellite_prn Global satellite ID - * @param satellite_used 0: Satellite not used, 1: used for localization - * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite - * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. - * @param satellite_snr Signal to noise ratio of satellite - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t satellites_visible, const uint8_t *satellite_prn, const uint8_t *satellite_used, const uint8_t *satellite_elevation, const uint8_t *satellite_azimuth, const uint8_t *satellite_snr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS_STATUS_LEN]; - _mav_put_uint8_t(buf, 0, satellites_visible); - _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); - _mav_put_uint8_t_array(buf, 21, satellite_used, 20); - _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); - _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); - _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_STATUS_LEN); -#else - mavlink_gps_status_t packet; - packet.satellites_visible = satellites_visible; - mav_array_memcpy(packet.satellite_prn, satellite_prn, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_used, satellite_used, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_snr, satellite_snr, sizeof(uint8_t)*20); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_LEN); -#endif -} - -/** - * @brief Pack a gps_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param satellites_visible Number of satellites visible - * @param satellite_prn Global satellite ID - * @param satellite_used 0: Satellite not used, 1: used for localization - * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite - * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. - * @param satellite_snr Signal to noise ratio of satellite - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t satellites_visible,const uint8_t *satellite_prn,const uint8_t *satellite_used,const uint8_t *satellite_elevation,const uint8_t *satellite_azimuth,const uint8_t *satellite_snr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS_STATUS_LEN]; - _mav_put_uint8_t(buf, 0, satellites_visible); - _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); - _mav_put_uint8_t_array(buf, 21, satellite_used, 20); - _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); - _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); - _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_STATUS_LEN); -#else - mavlink_gps_status_t packet; - packet.satellites_visible = satellites_visible; - mav_array_memcpy(packet.satellite_prn, satellite_prn, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_used, satellite_used, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_snr, satellite_snr, sizeof(uint8_t)*20); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_STATUS_LEN); -#endif -} - -/** - * @brief Encode a gps_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param gps_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_status_t* gps_status) -{ - return mavlink_msg_gps_status_pack(system_id, component_id, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); -} - -/** - * @brief Encode a gps_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param gps_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_status_t* gps_status) -{ - return mavlink_msg_gps_status_pack_chan(system_id, component_id, chan, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); -} - -/** - * @brief Send a gps_status message - * @param chan MAVLink channel to send the message - * - * @param satellites_visible Number of satellites visible - * @param satellite_prn Global satellite ID - * @param satellite_used 0: Satellite not used, 1: used for localization - * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite - * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. - * @param satellite_snr Signal to noise ratio of satellite - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t *satellite_prn, const uint8_t *satellite_used, const uint8_t *satellite_elevation, const uint8_t *satellite_azimuth, const uint8_t *satellite_snr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GPS_STATUS_LEN]; - _mav_put_uint8_t(buf, 0, satellites_visible); - _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); - _mav_put_uint8_t_array(buf, 21, satellite_used, 20); - _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); - _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); - _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN); -#endif -#else - mavlink_gps_status_t packet; - packet.satellites_visible = satellites_visible; - mav_array_memcpy(packet.satellite_prn, satellite_prn, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_used, satellite_used, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_snr, satellite_snr, sizeof(uint8_t)*20); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_GPS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_gps_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t *satellite_prn, const uint8_t *satellite_used, const uint8_t *satellite_elevation, const uint8_t *satellite_azimuth, const uint8_t *satellite_snr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, satellites_visible); - _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); - _mav_put_uint8_t_array(buf, 21, satellite_used, 20); - _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); - _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); - _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN); -#endif -#else - mavlink_gps_status_t *packet = (mavlink_gps_status_t *)msgbuf; - packet->satellites_visible = satellites_visible; - mav_array_memcpy(packet->satellite_prn, satellite_prn, sizeof(uint8_t)*20); - mav_array_memcpy(packet->satellite_used, satellite_used, sizeof(uint8_t)*20); - mav_array_memcpy(packet->satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); - mav_array_memcpy(packet->satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); - mav_array_memcpy(packet->satellite_snr, satellite_snr, sizeof(uint8_t)*20); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE GPS_STATUS UNPACKING - - -/** - * @brief Get field satellites_visible from gps_status message - * - * @return Number of satellites visible - */ -static inline uint8_t mavlink_msg_gps_status_get_satellites_visible(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field satellite_prn from gps_status message - * - * @return Global satellite ID - */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_message_t* msg, uint8_t *satellite_prn) -{ - return _MAV_RETURN_uint8_t_array(msg, satellite_prn, 20, 1); -} - -/** - * @brief Get field satellite_used from gps_status message - * - * @return 0: Satellite not used, 1: used for localization - */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_message_t* msg, uint8_t *satellite_used) -{ - return _MAV_RETURN_uint8_t_array(msg, satellite_used, 20, 21); -} - -/** - * @brief Get field satellite_elevation from gps_status message - * - * @return Elevation (0: right on top of receiver, 90: on the horizon) of satellite - */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavlink_message_t* msg, uint8_t *satellite_elevation) -{ - return _MAV_RETURN_uint8_t_array(msg, satellite_elevation, 20, 41); -} - -/** - * @brief Get field satellite_azimuth from gps_status message - * - * @return Direction of satellite, 0: 0 deg, 255: 360 deg. - */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlink_message_t* msg, uint8_t *satellite_azimuth) -{ - return _MAV_RETURN_uint8_t_array(msg, satellite_azimuth, 20, 61); -} - -/** - * @brief Get field satellite_snr from gps_status message - * - * @return Signal to noise ratio of satellite - */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_message_t* msg, uint8_t *satellite_snr) -{ - return _MAV_RETURN_uint8_t_array(msg, satellite_snr, 20, 81); -} - -/** - * @brief Decode a gps_status message into a struct - * - * @param msg The message to decode - * @param gps_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_gps_status_decode(const mavlink_message_t* msg, mavlink_gps_status_t* gps_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - gps_status->satellites_visible = mavlink_msg_gps_status_get_satellites_visible(msg); - mavlink_msg_gps_status_get_satellite_prn(msg, gps_status->satellite_prn); - mavlink_msg_gps_status_get_satellite_used(msg, gps_status->satellite_used); - mavlink_msg_gps_status_get_satellite_elevation(msg, gps_status->satellite_elevation); - mavlink_msg_gps_status_get_satellite_azimuth(msg, gps_status->satellite_azimuth); - mavlink_msg_gps_status_get_satellite_snr(msg, gps_status->satellite_snr); -#else - memcpy(gps_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h deleted file mode 100644 index 902e381ca..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h +++ /dev/null @@ -1,326 +0,0 @@ -// MESSAGE HEARTBEAT PACKING - -#define MAVLINK_MSG_ID_HEARTBEAT 0 - -typedef struct __mavlink_heartbeat_t -{ - uint32_t custom_mode; ///< A bitfield for use for autopilot-specific flags. - uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - uint8_t autopilot; ///< Autopilot type / class. defined in MAV_AUTOPILOT ENUM - uint8_t base_mode; ///< System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - uint8_t system_status; ///< System status flag, see MAV_STATE ENUM - uint8_t mavlink_version; ///< MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version -} mavlink_heartbeat_t; - -#define MAVLINK_MSG_ID_HEARTBEAT_LEN 9 -#define MAVLINK_MSG_ID_0_LEN 9 - -#define MAVLINK_MSG_ID_HEARTBEAT_CRC 50 -#define MAVLINK_MSG_ID_0_CRC 50 - - - -#define MAVLINK_MESSAGE_INFO_HEARTBEAT { \ - "HEARTBEAT", \ - 6, \ - { { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_heartbeat_t, custom_mode) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_heartbeat_t, type) }, \ - { "autopilot", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_heartbeat_t, autopilot) }, \ - { "base_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_heartbeat_t, base_mode) }, \ - { "system_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_heartbeat_t, system_status) }, \ - { "mavlink_version", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_heartbeat_t, mavlink_version) }, \ - } \ -} - - -/** - * @brief Pack a heartbeat message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - * @param base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - * @param custom_mode A bitfield for use for autopilot-specific flags. - * @param system_status System status flag, see MAV_STATE ENUM - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 5, autopilot); - _mav_put_uint8_t(buf, 6, base_mode); - _mav_put_uint8_t(buf, 7, system_status); - _mav_put_uint8_t(buf, 8, 3); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#else - mavlink_heartbeat_t packet; - packet.custom_mode = custom_mode; - packet.type = type; - packet.autopilot = autopilot; - packet.base_mode = base_mode; - packet.system_status = system_status; - packet.mavlink_version = 3; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif -} - -/** - * @brief Pack a heartbeat message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - * @param base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - * @param custom_mode A bitfield for use for autopilot-specific flags. - * @param system_status System status flag, see MAV_STATE ENUM - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t autopilot,uint8_t base_mode,uint32_t custom_mode,uint8_t system_status) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 5, autopilot); - _mav_put_uint8_t(buf, 6, base_mode); - _mav_put_uint8_t(buf, 7, system_status); - _mav_put_uint8_t(buf, 8, 3); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#else - mavlink_heartbeat_t packet; - packet.custom_mode = custom_mode; - packet.type = type; - packet.autopilot = autopilot; - packet.base_mode = base_mode; - packet.system_status = system_status; - packet.mavlink_version = 3; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif -} - -/** - * @brief Encode a heartbeat struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param heartbeat C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) -{ - return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); -} - -/** - * @brief Encode a heartbeat struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param heartbeat C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) -{ - return mavlink_msg_heartbeat_pack_chan(system_id, component_id, chan, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); -} - -/** - * @brief Send a heartbeat message - * @param chan MAVLink channel to send the message - * - * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - * @param base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - * @param custom_mode A bitfield for use for autopilot-specific flags. - * @param system_status System status flag, see MAV_STATE ENUM - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 5, autopilot); - _mav_put_uint8_t(buf, 6, base_mode); - _mav_put_uint8_t(buf, 7, system_status); - _mav_put_uint8_t(buf, 8, 3); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif -#else - mavlink_heartbeat_t packet; - packet.custom_mode = custom_mode; - packet.type = type; - packet.autopilot = autopilot; - packet.base_mode = base_mode; - packet.system_status = system_status; - packet.mavlink_version = 3; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 5, autopilot); - _mav_put_uint8_t(buf, 6, base_mode); - _mav_put_uint8_t(buf, 7, system_status); - _mav_put_uint8_t(buf, 8, 3); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif -#else - mavlink_heartbeat_t *packet = (mavlink_heartbeat_t *)msgbuf; - packet->custom_mode = custom_mode; - packet->type = type; - packet->autopilot = autopilot; - packet->base_mode = base_mode; - packet->system_status = system_status; - packet->mavlink_version = 3; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE HEARTBEAT UNPACKING - - -/** - * @brief Get field type from heartbeat message - * - * @return Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - */ -static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field autopilot from heartbeat message - * - * @return Autopilot type / class. defined in MAV_AUTOPILOT ENUM - */ -static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field base_mode from heartbeat message - * - * @return System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - */ -static inline uint8_t mavlink_msg_heartbeat_get_base_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field custom_mode from heartbeat message - * - * @return A bitfield for use for autopilot-specific flags. - */ -static inline uint32_t mavlink_msg_heartbeat_get_custom_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field system_status from heartbeat message - * - * @return System status flag, see MAV_STATE ENUM - */ -static inline uint8_t mavlink_msg_heartbeat_get_system_status(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field mavlink_version from heartbeat message - * - * @return MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version - */ -static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Decode a heartbeat message into a struct - * - * @param msg The message to decode - * @param heartbeat C-struct to decode the message contents into - */ -static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat) -{ -#if MAVLINK_NEED_BYTE_SWAP - heartbeat->custom_mode = mavlink_msg_heartbeat_get_custom_mode(msg); - heartbeat->type = mavlink_msg_heartbeat_get_type(msg); - heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); - heartbeat->base_mode = mavlink_msg_heartbeat_get_base_mode(msg); - heartbeat->system_status = mavlink_msg_heartbeat_get_system_status(msg); - heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); -#else - memcpy(heartbeat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h deleted file mode 100644 index 2749cb097..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h +++ /dev/null @@ -1,545 +0,0 @@ -// MESSAGE HIGHRES_IMU PACKING - -#define MAVLINK_MSG_ID_HIGHRES_IMU 105 - -typedef struct __mavlink_highres_imu_t -{ - uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) - float xacc; ///< X acceleration (m/s^2) - float yacc; ///< Y acceleration (m/s^2) - float zacc; ///< Z acceleration (m/s^2) - float xgyro; ///< Angular speed around X axis (rad / sec) - float ygyro; ///< Angular speed around Y axis (rad / sec) - float zgyro; ///< Angular speed around Z axis (rad / sec) - float xmag; ///< X Magnetic field (Gauss) - float ymag; ///< Y Magnetic field (Gauss) - float zmag; ///< Z Magnetic field (Gauss) - float abs_pressure; ///< Absolute pressure in millibar - float diff_pressure; ///< Differential pressure in millibar - float pressure_alt; ///< Altitude calculated from pressure - float temperature; ///< Temperature in degrees celsius - uint16_t fields_updated; ///< Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature -} mavlink_highres_imu_t; - -#define MAVLINK_MSG_ID_HIGHRES_IMU_LEN 62 -#define MAVLINK_MSG_ID_105_LEN 62 - -#define MAVLINK_MSG_ID_HIGHRES_IMU_CRC 93 -#define MAVLINK_MSG_ID_105_CRC 93 - - - -#define MAVLINK_MESSAGE_INFO_HIGHRES_IMU { \ - "HIGHRES_IMU", \ - 15, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_highres_imu_t, time_usec) }, \ - { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_highres_imu_t, xacc) }, \ - { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_highres_imu_t, yacc) }, \ - { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_highres_imu_t, zacc) }, \ - { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_highres_imu_t, xgyro) }, \ - { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_highres_imu_t, ygyro) }, \ - { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_highres_imu_t, zgyro) }, \ - { "xmag", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_highres_imu_t, xmag) }, \ - { "ymag", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_highres_imu_t, ymag) }, \ - { "zmag", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_highres_imu_t, zmag) }, \ - { "abs_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_highres_imu_t, abs_pressure) }, \ - { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_highres_imu_t, diff_pressure) }, \ - { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_highres_imu_t, pressure_alt) }, \ - { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_highres_imu_t, temperature) }, \ - { "fields_updated", NULL, MAVLINK_TYPE_UINT16_T, 0, 60, offsetof(mavlink_highres_imu_t, fields_updated) }, \ - } \ -} - - -/** - * @brief Pack a highres_imu message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param xacc X acceleration (m/s^2) - * @param yacc Y acceleration (m/s^2) - * @param zacc Z acceleration (m/s^2) - * @param xgyro Angular speed around X axis (rad / sec) - * @param ygyro Angular speed around Y axis (rad / sec) - * @param zgyro Angular speed around Z axis (rad / sec) - * @param xmag X Magnetic field (Gauss) - * @param ymag Y Magnetic field (Gauss) - * @param zmag Z Magnetic field (Gauss) - * @param abs_pressure Absolute pressure in millibar - * @param diff_pressure Differential pressure in millibar - * @param pressure_alt Altitude calculated from pressure - * @param temperature Temperature in degrees celsius - * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, xacc); - _mav_put_float(buf, 12, yacc); - _mav_put_float(buf, 16, zacc); - _mav_put_float(buf, 20, xgyro); - _mav_put_float(buf, 24, ygyro); - _mav_put_float(buf, 28, zgyro); - _mav_put_float(buf, 32, xmag); - _mav_put_float(buf, 36, ymag); - _mav_put_float(buf, 40, zmag); - _mav_put_float(buf, 44, abs_pressure); - _mav_put_float(buf, 48, diff_pressure); - _mav_put_float(buf, 52, pressure_alt); - _mav_put_float(buf, 56, temperature); - _mav_put_uint16_t(buf, 60, fields_updated); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); -#else - mavlink_highres_imu_t packet; - packet.time_usec = time_usec; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - packet.abs_pressure = abs_pressure; - packet.diff_pressure = diff_pressure; - packet.pressure_alt = pressure_alt; - packet.temperature = temperature; - packet.fields_updated = fields_updated; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIGHRES_IMU; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); -#endif -} - -/** - * @brief Pack a highres_imu message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param xacc X acceleration (m/s^2) - * @param yacc Y acceleration (m/s^2) - * @param zacc Z acceleration (m/s^2) - * @param xgyro Angular speed around X axis (rad / sec) - * @param ygyro Angular speed around Y axis (rad / sec) - * @param zgyro Angular speed around Z axis (rad / sec) - * @param xmag X Magnetic field (Gauss) - * @param ymag Y Magnetic field (Gauss) - * @param zmag Z Magnetic field (Gauss) - * @param abs_pressure Absolute pressure in millibar - * @param diff_pressure Differential pressure in millibar - * @param pressure_alt Altitude calculated from pressure - * @param temperature Temperature in degrees celsius - * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint16_t fields_updated) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, xacc); - _mav_put_float(buf, 12, yacc); - _mav_put_float(buf, 16, zacc); - _mav_put_float(buf, 20, xgyro); - _mav_put_float(buf, 24, ygyro); - _mav_put_float(buf, 28, zgyro); - _mav_put_float(buf, 32, xmag); - _mav_put_float(buf, 36, ymag); - _mav_put_float(buf, 40, zmag); - _mav_put_float(buf, 44, abs_pressure); - _mav_put_float(buf, 48, diff_pressure); - _mav_put_float(buf, 52, pressure_alt); - _mav_put_float(buf, 56, temperature); - _mav_put_uint16_t(buf, 60, fields_updated); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); -#else - mavlink_highres_imu_t packet; - packet.time_usec = time_usec; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - packet.abs_pressure = abs_pressure; - packet.diff_pressure = diff_pressure; - packet.pressure_alt = pressure_alt; - packet.temperature = temperature; - packet.fields_updated = fields_updated; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIGHRES_IMU; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); -#endif -} - -/** - * @brief Encode a highres_imu struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param highres_imu C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu) -{ - return mavlink_msg_highres_imu_pack(system_id, component_id, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); -} - -/** - * @brief Encode a highres_imu struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param highres_imu C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_highres_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu) -{ - return mavlink_msg_highres_imu_pack_chan(system_id, component_id, chan, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); -} - -/** - * @brief Send a highres_imu message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param xacc X acceleration (m/s^2) - * @param yacc Y acceleration (m/s^2) - * @param zacc Z acceleration (m/s^2) - * @param xgyro Angular speed around X axis (rad / sec) - * @param ygyro Angular speed around Y axis (rad / sec) - * @param zgyro Angular speed around Z axis (rad / sec) - * @param xmag X Magnetic field (Gauss) - * @param ymag Y Magnetic field (Gauss) - * @param zmag Z Magnetic field (Gauss) - * @param abs_pressure Absolute pressure in millibar - * @param diff_pressure Differential pressure in millibar - * @param pressure_alt Altitude calculated from pressure - * @param temperature Temperature in degrees celsius - * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, xacc); - _mav_put_float(buf, 12, yacc); - _mav_put_float(buf, 16, zacc); - _mav_put_float(buf, 20, xgyro); - _mav_put_float(buf, 24, ygyro); - _mav_put_float(buf, 28, zgyro); - _mav_put_float(buf, 32, xmag); - _mav_put_float(buf, 36, ymag); - _mav_put_float(buf, 40, zmag); - _mav_put_float(buf, 44, abs_pressure); - _mav_put_float(buf, 48, diff_pressure); - _mav_put_float(buf, 52, pressure_alt); - _mav_put_float(buf, 56, temperature); - _mav_put_uint16_t(buf, 60, fields_updated); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); -#endif -#else - mavlink_highres_imu_t packet; - packet.time_usec = time_usec; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - packet.abs_pressure = abs_pressure; - packet.diff_pressure = diff_pressure; - packet.pressure_alt = pressure_alt; - packet.temperature = temperature; - packet.fields_updated = fields_updated; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)&packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)&packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_HIGHRES_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, xacc); - _mav_put_float(buf, 12, yacc); - _mav_put_float(buf, 16, zacc); - _mav_put_float(buf, 20, xgyro); - _mav_put_float(buf, 24, ygyro); - _mav_put_float(buf, 28, zgyro); - _mav_put_float(buf, 32, xmag); - _mav_put_float(buf, 36, ymag); - _mav_put_float(buf, 40, zmag); - _mav_put_float(buf, 44, abs_pressure); - _mav_put_float(buf, 48, diff_pressure); - _mav_put_float(buf, 52, pressure_alt); - _mav_put_float(buf, 56, temperature); - _mav_put_uint16_t(buf, 60, fields_updated); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); -#endif -#else - mavlink_highres_imu_t *packet = (mavlink_highres_imu_t *)msgbuf; - packet->time_usec = time_usec; - packet->xacc = xacc; - packet->yacc = yacc; - packet->zacc = zacc; - packet->xgyro = xgyro; - packet->ygyro = ygyro; - packet->zgyro = zgyro; - packet->xmag = xmag; - packet->ymag = ymag; - packet->zmag = zmag; - packet->abs_pressure = abs_pressure; - packet->diff_pressure = diff_pressure; - packet->pressure_alt = pressure_alt; - packet->temperature = temperature; - packet->fields_updated = fields_updated; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE HIGHRES_IMU UNPACKING - - -/** - * @brief Get field time_usec from highres_imu message - * - * @return Timestamp (microseconds, synced to UNIX time or since system boot) - */ -static inline uint64_t mavlink_msg_highres_imu_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field xacc from highres_imu message - * - * @return X acceleration (m/s^2) - */ -static inline float mavlink_msg_highres_imu_get_xacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yacc from highres_imu message - * - * @return Y acceleration (m/s^2) - */ -static inline float mavlink_msg_highres_imu_get_yacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field zacc from highres_imu message - * - * @return Z acceleration (m/s^2) - */ -static inline float mavlink_msg_highres_imu_get_zacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field xgyro from highres_imu message - * - * @return Angular speed around X axis (rad / sec) - */ -static inline float mavlink_msg_highres_imu_get_xgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field ygyro from highres_imu message - * - * @return Angular speed around Y axis (rad / sec) - */ -static inline float mavlink_msg_highres_imu_get_ygyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field zgyro from highres_imu message - * - * @return Angular speed around Z axis (rad / sec) - */ -static inline float mavlink_msg_highres_imu_get_zgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field xmag from highres_imu message - * - * @return X Magnetic field (Gauss) - */ -static inline float mavlink_msg_highres_imu_get_xmag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field ymag from highres_imu message - * - * @return Y Magnetic field (Gauss) - */ -static inline float mavlink_msg_highres_imu_get_ymag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 36); -} - -/** - * @brief Get field zmag from highres_imu message - * - * @return Z Magnetic field (Gauss) - */ -static inline float mavlink_msg_highres_imu_get_zmag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 40); -} - -/** - * @brief Get field abs_pressure from highres_imu message - * - * @return Absolute pressure in millibar - */ -static inline float mavlink_msg_highres_imu_get_abs_pressure(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 44); -} - -/** - * @brief Get field diff_pressure from highres_imu message - * - * @return Differential pressure in millibar - */ -static inline float mavlink_msg_highres_imu_get_diff_pressure(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 48); -} - -/** - * @brief Get field pressure_alt from highres_imu message - * - * @return Altitude calculated from pressure - */ -static inline float mavlink_msg_highres_imu_get_pressure_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 52); -} - -/** - * @brief Get field temperature from highres_imu message - * - * @return Temperature in degrees celsius - */ -static inline float mavlink_msg_highres_imu_get_temperature(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 56); -} - -/** - * @brief Get field fields_updated from highres_imu message - * - * @return Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature - */ -static inline uint16_t mavlink_msg_highres_imu_get_fields_updated(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 60); -} - -/** - * @brief Decode a highres_imu message into a struct - * - * @param msg The message to decode - * @param highres_imu C-struct to decode the message contents into - */ -static inline void mavlink_msg_highres_imu_decode(const mavlink_message_t* msg, mavlink_highres_imu_t* highres_imu) -{ -#if MAVLINK_NEED_BYTE_SWAP - highres_imu->time_usec = mavlink_msg_highres_imu_get_time_usec(msg); - highres_imu->xacc = mavlink_msg_highres_imu_get_xacc(msg); - highres_imu->yacc = mavlink_msg_highres_imu_get_yacc(msg); - highres_imu->zacc = mavlink_msg_highres_imu_get_zacc(msg); - highres_imu->xgyro = mavlink_msg_highres_imu_get_xgyro(msg); - highres_imu->ygyro = mavlink_msg_highres_imu_get_ygyro(msg); - highres_imu->zgyro = mavlink_msg_highres_imu_get_zgyro(msg); - highres_imu->xmag = mavlink_msg_highres_imu_get_xmag(msg); - highres_imu->ymag = mavlink_msg_highres_imu_get_ymag(msg); - highres_imu->zmag = mavlink_msg_highres_imu_get_zmag(msg); - highres_imu->abs_pressure = mavlink_msg_highres_imu_get_abs_pressure(msg); - highres_imu->diff_pressure = mavlink_msg_highres_imu_get_diff_pressure(msg); - highres_imu->pressure_alt = mavlink_msg_highres_imu_get_pressure_alt(msg); - highres_imu->temperature = mavlink_msg_highres_imu_get_temperature(msg); - highres_imu->fields_updated = mavlink_msg_highres_imu_get_fields_updated(msg); -#else - memcpy(highres_imu, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIGHRES_IMU_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h deleted file mode 100644 index f7507b1cf..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h +++ /dev/null @@ -1,449 +0,0 @@ -// MESSAGE HIL_CONTROLS PACKING - -#define MAVLINK_MSG_ID_HIL_CONTROLS 91 - -typedef struct __mavlink_hil_controls_t -{ - uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float roll_ailerons; ///< Control output -1 .. 1 - float pitch_elevator; ///< Control output -1 .. 1 - float yaw_rudder; ///< Control output -1 .. 1 - float throttle; ///< Throttle 0 .. 1 - float aux1; ///< Aux 1, -1 .. 1 - float aux2; ///< Aux 2, -1 .. 1 - float aux3; ///< Aux 3, -1 .. 1 - float aux4; ///< Aux 4, -1 .. 1 - uint8_t mode; ///< System mode (MAV_MODE) - uint8_t nav_mode; ///< Navigation mode (MAV_NAV_MODE) -} mavlink_hil_controls_t; - -#define MAVLINK_MSG_ID_HIL_CONTROLS_LEN 42 -#define MAVLINK_MSG_ID_91_LEN 42 - -#define MAVLINK_MSG_ID_HIL_CONTROLS_CRC 63 -#define MAVLINK_MSG_ID_91_CRC 63 - - - -#define MAVLINK_MESSAGE_INFO_HIL_CONTROLS { \ - "HIL_CONTROLS", \ - 11, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_controls_t, time_usec) }, \ - { "roll_ailerons", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_controls_t, roll_ailerons) }, \ - { "pitch_elevator", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_controls_t, pitch_elevator) }, \ - { "yaw_rudder", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_controls_t, yaw_rudder) }, \ - { "throttle", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_controls_t, throttle) }, \ - { "aux1", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_controls_t, aux1) }, \ - { "aux2", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_controls_t, aux2) }, \ - { "aux3", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_hil_controls_t, aux3) }, \ - { "aux4", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_hil_controls_t, aux4) }, \ - { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_hil_controls_t, mode) }, \ - { "nav_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_hil_controls_t, nav_mode) }, \ - } \ -} - - -/** - * @brief Pack a hil_controls message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll_ailerons Control output -1 .. 1 - * @param pitch_elevator Control output -1 .. 1 - * @param yaw_rudder Control output -1 .. 1 - * @param throttle Throttle 0 .. 1 - * @param aux1 Aux 1, -1 .. 1 - * @param aux2 Aux 2, -1 .. 1 - * @param aux3 Aux 3, -1 .. 1 - * @param aux4 Aux 4, -1 .. 1 - * @param mode System mode (MAV_MODE) - * @param nav_mode Navigation mode (MAV_NAV_MODE) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_CONTROLS_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, roll_ailerons); - _mav_put_float(buf, 12, pitch_elevator); - _mav_put_float(buf, 16, yaw_rudder); - _mav_put_float(buf, 20, throttle); - _mav_put_float(buf, 24, aux1); - _mav_put_float(buf, 28, aux2); - _mav_put_float(buf, 32, aux3); - _mav_put_float(buf, 36, aux4); - _mav_put_uint8_t(buf, 40, mode); - _mav_put_uint8_t(buf, 41, nav_mode); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); -#else - mavlink_hil_controls_t packet; - packet.time_usec = time_usec; - packet.roll_ailerons = roll_ailerons; - packet.pitch_elevator = pitch_elevator; - packet.yaw_rudder = yaw_rudder; - packet.throttle = throttle; - packet.aux1 = aux1; - packet.aux2 = aux2; - packet.aux3 = aux3; - packet.aux4 = aux4; - packet.mode = mode; - packet.nav_mode = nav_mode; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); -#endif -} - -/** - * @brief Pack a hil_controls message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll_ailerons Control output -1 .. 1 - * @param pitch_elevator Control output -1 .. 1 - * @param yaw_rudder Control output -1 .. 1 - * @param throttle Throttle 0 .. 1 - * @param aux1 Aux 1, -1 .. 1 - * @param aux2 Aux 2, -1 .. 1 - * @param aux3 Aux 3, -1 .. 1 - * @param aux4 Aux 4, -1 .. 1 - * @param mode System mode (MAV_MODE) - * @param nav_mode Navigation mode (MAV_NAV_MODE) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,float roll_ailerons,float pitch_elevator,float yaw_rudder,float throttle,float aux1,float aux2,float aux3,float aux4,uint8_t mode,uint8_t nav_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_CONTROLS_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, roll_ailerons); - _mav_put_float(buf, 12, pitch_elevator); - _mav_put_float(buf, 16, yaw_rudder); - _mav_put_float(buf, 20, throttle); - _mav_put_float(buf, 24, aux1); - _mav_put_float(buf, 28, aux2); - _mav_put_float(buf, 32, aux3); - _mav_put_float(buf, 36, aux4); - _mav_put_uint8_t(buf, 40, mode); - _mav_put_uint8_t(buf, 41, nav_mode); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); -#else - mavlink_hil_controls_t packet; - packet.time_usec = time_usec; - packet.roll_ailerons = roll_ailerons; - packet.pitch_elevator = pitch_elevator; - packet.yaw_rudder = yaw_rudder; - packet.throttle = throttle; - packet.aux1 = aux1; - packet.aux2 = aux2; - packet.aux3 = aux3; - packet.aux4 = aux4; - packet.mode = mode; - packet.nav_mode = nav_mode; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); -#endif -} - -/** - * @brief Encode a hil_controls struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param hil_controls C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls) -{ - return mavlink_msg_hil_controls_pack(system_id, component_id, msg, hil_controls->time_usec, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode); -} - -/** - * @brief Encode a hil_controls struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param hil_controls C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_controls_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls) -{ - return mavlink_msg_hil_controls_pack_chan(system_id, component_id, chan, msg, hil_controls->time_usec, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode); -} - -/** - * @brief Send a hil_controls message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll_ailerons Control output -1 .. 1 - * @param pitch_elevator Control output -1 .. 1 - * @param yaw_rudder Control output -1 .. 1 - * @param throttle Throttle 0 .. 1 - * @param aux1 Aux 1, -1 .. 1 - * @param aux2 Aux 2, -1 .. 1 - * @param aux3 Aux 3, -1 .. 1 - * @param aux4 Aux 4, -1 .. 1 - * @param mode System mode (MAV_MODE) - * @param nav_mode Navigation mode (MAV_NAV_MODE) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_CONTROLS_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, roll_ailerons); - _mav_put_float(buf, 12, pitch_elevator); - _mav_put_float(buf, 16, yaw_rudder); - _mav_put_float(buf, 20, throttle); - _mav_put_float(buf, 24, aux1); - _mav_put_float(buf, 28, aux2); - _mav_put_float(buf, 32, aux3); - _mav_put_float(buf, 36, aux4); - _mav_put_uint8_t(buf, 40, mode); - _mav_put_uint8_t(buf, 41, nav_mode); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); -#endif -#else - mavlink_hil_controls_t packet; - packet.time_usec = time_usec; - packet.roll_ailerons = roll_ailerons; - packet.pitch_elevator = pitch_elevator; - packet.yaw_rudder = yaw_rudder; - packet.throttle = throttle; - packet.aux1 = aux1; - packet.aux2 = aux2; - packet.aux3 = aux3; - packet.aux4 = aux4; - packet.mode = mode; - packet.nav_mode = nav_mode; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)&packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)&packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_HIL_CONTROLS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_hil_controls_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, roll_ailerons); - _mav_put_float(buf, 12, pitch_elevator); - _mav_put_float(buf, 16, yaw_rudder); - _mav_put_float(buf, 20, throttle); - _mav_put_float(buf, 24, aux1); - _mav_put_float(buf, 28, aux2); - _mav_put_float(buf, 32, aux3); - _mav_put_float(buf, 36, aux4); - _mav_put_uint8_t(buf, 40, mode); - _mav_put_uint8_t(buf, 41, nav_mode); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); -#endif -#else - mavlink_hil_controls_t *packet = (mavlink_hil_controls_t *)msgbuf; - packet->time_usec = time_usec; - packet->roll_ailerons = roll_ailerons; - packet->pitch_elevator = pitch_elevator; - packet->yaw_rudder = yaw_rudder; - packet->throttle = throttle; - packet->aux1 = aux1; - packet->aux2 = aux2; - packet->aux3 = aux3; - packet->aux4 = aux4; - packet->mode = mode; - packet->nav_mode = nav_mode; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE HIL_CONTROLS UNPACKING - - -/** - * @brief Get field time_usec from hil_controls message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_hil_controls_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field roll_ailerons from hil_controls message - * - * @return Control output -1 .. 1 - */ -static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field pitch_elevator from hil_controls message - * - * @return Control output -1 .. 1 - */ -static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field yaw_rudder from hil_controls message - * - * @return Control output -1 .. 1 - */ -static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field throttle from hil_controls message - * - * @return Throttle 0 .. 1 - */ -static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field aux1 from hil_controls message - * - * @return Aux 1, -1 .. 1 - */ -static inline float mavlink_msg_hil_controls_get_aux1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field aux2 from hil_controls message - * - * @return Aux 2, -1 .. 1 - */ -static inline float mavlink_msg_hil_controls_get_aux2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field aux3 from hil_controls message - * - * @return Aux 3, -1 .. 1 - */ -static inline float mavlink_msg_hil_controls_get_aux3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field aux4 from hil_controls message - * - * @return Aux 4, -1 .. 1 - */ -static inline float mavlink_msg_hil_controls_get_aux4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 36); -} - -/** - * @brief Get field mode from hil_controls message - * - * @return System mode (MAV_MODE) - */ -static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 40); -} - -/** - * @brief Get field nav_mode from hil_controls message - * - * @return Navigation mode (MAV_NAV_MODE) - */ -static inline uint8_t mavlink_msg_hil_controls_get_nav_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 41); -} - -/** - * @brief Decode a hil_controls message into a struct - * - * @param msg The message to decode - * @param hil_controls C-struct to decode the message contents into - */ -static inline void mavlink_msg_hil_controls_decode(const mavlink_message_t* msg, mavlink_hil_controls_t* hil_controls) -{ -#if MAVLINK_NEED_BYTE_SWAP - hil_controls->time_usec = mavlink_msg_hil_controls_get_time_usec(msg); - hil_controls->roll_ailerons = mavlink_msg_hil_controls_get_roll_ailerons(msg); - hil_controls->pitch_elevator = mavlink_msg_hil_controls_get_pitch_elevator(msg); - hil_controls->yaw_rudder = mavlink_msg_hil_controls_get_yaw_rudder(msg); - hil_controls->throttle = mavlink_msg_hil_controls_get_throttle(msg); - hil_controls->aux1 = mavlink_msg_hil_controls_get_aux1(msg); - hil_controls->aux2 = mavlink_msg_hil_controls_get_aux2(msg); - hil_controls->aux3 = mavlink_msg_hil_controls_get_aux3(msg); - hil_controls->aux4 = mavlink_msg_hil_controls_get_aux4(msg); - hil_controls->mode = mavlink_msg_hil_controls_get_mode(msg); - hil_controls->nav_mode = mavlink_msg_hil_controls_get_nav_mode(msg); -#else - memcpy(hil_controls, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_CONTROLS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h deleted file mode 100644 index a22c0472f..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h +++ /dev/null @@ -1,497 +0,0 @@ -// MESSAGE HIL_GPS PACKING - -#define MAVLINK_MSG_ID_HIL_GPS 113 - -typedef struct __mavlink_hil_gps_t -{ - uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int32_t lat; ///< Latitude (WGS84), in degrees * 1E7 - int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 - int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up) - uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535 - int16_t vn; ///< GPS velocity in cm/s in NORTH direction in earth-fixed NED frame - int16_t ve; ///< GPS velocity in cm/s in EAST direction in earth-fixed NED frame - int16_t vd; ///< GPS velocity in cm/s in DOWN direction in earth-fixed NED frame - uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 -} mavlink_hil_gps_t; - -#define MAVLINK_MSG_ID_HIL_GPS_LEN 36 -#define MAVLINK_MSG_ID_113_LEN 36 - -#define MAVLINK_MSG_ID_HIL_GPS_CRC 124 -#define MAVLINK_MSG_ID_113_CRC 124 - - - -#define MAVLINK_MESSAGE_INFO_HIL_GPS { \ - "HIL_GPS", \ - 13, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_gps_t, time_usec) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_hil_gps_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_hil_gps_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_hil_gps_t, alt) }, \ - { "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_hil_gps_t, eph) }, \ - { "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_hil_gps_t, epv) }, \ - { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_hil_gps_t, vel) }, \ - { "vn", NULL, MAVLINK_TYPE_INT16_T, 0, 26, offsetof(mavlink_hil_gps_t, vn) }, \ - { "ve", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_hil_gps_t, ve) }, \ - { "vd", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_hil_gps_t, vd) }, \ - { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_hil_gps_t, cog) }, \ - { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_hil_gps_t, fix_type) }, \ - { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_hil_gps_t, satellites_visible) }, \ - } \ -} - - -/** - * @brief Pack a hil_gps message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude (WGS84), in degrees * 1E7 - * @param lon Longitude (WGS84), in degrees * 1E7 - * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame - * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame - * @param vd GPS velocity in cm/s in DOWN direction in earth-fixed NED frame - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - * @param satellites_visible Number of satellites visible. If unknown, set to 255 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int32_t(buf, 8, lat); - _mav_put_int32_t(buf, 12, lon); - _mav_put_int32_t(buf, 16, alt); - _mav_put_uint16_t(buf, 20, eph); - _mav_put_uint16_t(buf, 22, epv); - _mav_put_uint16_t(buf, 24, vel); - _mav_put_int16_t(buf, 26, vn); - _mav_put_int16_t(buf, 28, ve); - _mav_put_int16_t(buf, 30, vd); - _mav_put_uint16_t(buf, 32, cog); - _mav_put_uint8_t(buf, 34, fix_type); - _mav_put_uint8_t(buf, 35, satellites_visible); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN); -#else - mavlink_hil_gps_t packet; - packet.time_usec = time_usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.eph = eph; - packet.epv = epv; - packet.vel = vel; - packet.vn = vn; - packet.ve = ve; - packet.vd = vd; - packet.cog = cog; - packet.fix_type = fix_type; - packet.satellites_visible = satellites_visible; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_GPS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_GPS_LEN); -#endif -} - -/** - * @brief Pack a hil_gps message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude (WGS84), in degrees * 1E7 - * @param lon Longitude (WGS84), in degrees * 1E7 - * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame - * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame - * @param vd GPS velocity in cm/s in DOWN direction in earth-fixed NED frame - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - * @param satellites_visible Number of satellites visible. If unknown, set to 255 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,int16_t vn,int16_t ve,int16_t vd,uint16_t cog,uint8_t satellites_visible) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int32_t(buf, 8, lat); - _mav_put_int32_t(buf, 12, lon); - _mav_put_int32_t(buf, 16, alt); - _mav_put_uint16_t(buf, 20, eph); - _mav_put_uint16_t(buf, 22, epv); - _mav_put_uint16_t(buf, 24, vel); - _mav_put_int16_t(buf, 26, vn); - _mav_put_int16_t(buf, 28, ve); - _mav_put_int16_t(buf, 30, vd); - _mav_put_uint16_t(buf, 32, cog); - _mav_put_uint8_t(buf, 34, fix_type); - _mav_put_uint8_t(buf, 35, satellites_visible); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN); -#else - mavlink_hil_gps_t packet; - packet.time_usec = time_usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.eph = eph; - packet.epv = epv; - packet.vel = vel; - packet.vn = vn; - packet.ve = ve; - packet.vd = vd; - packet.cog = cog; - packet.fix_type = fix_type; - packet.satellites_visible = satellites_visible; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_GPS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_GPS_LEN); -#endif -} - -/** - * @brief Encode a hil_gps struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param hil_gps C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_gps_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) -{ - return mavlink_msg_hil_gps_pack(system_id, component_id, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); -} - -/** - * @brief Encode a hil_gps struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param hil_gps C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) -{ - return mavlink_msg_hil_gps_pack_chan(system_id, component_id, chan, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); -} - -/** - * @brief Send a hil_gps message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude (WGS84), in degrees * 1E7 - * @param lon Longitude (WGS84), in degrees * 1E7 - * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame - * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame - * @param vd GPS velocity in cm/s in DOWN direction in earth-fixed NED frame - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - * @param satellites_visible Number of satellites visible. If unknown, set to 255 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int32_t(buf, 8, lat); - _mav_put_int32_t(buf, 12, lon); - _mav_put_int32_t(buf, 16, alt); - _mav_put_uint16_t(buf, 20, eph); - _mav_put_uint16_t(buf, 22, epv); - _mav_put_uint16_t(buf, 24, vel); - _mav_put_int16_t(buf, 26, vn); - _mav_put_int16_t(buf, 28, ve); - _mav_put_int16_t(buf, 30, vd); - _mav_put_uint16_t(buf, 32, cog); - _mav_put_uint8_t(buf, 34, fix_type); - _mav_put_uint8_t(buf, 35, satellites_visible); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN); -#endif -#else - mavlink_hil_gps_t packet; - packet.time_usec = time_usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.eph = eph; - packet.epv = epv; - packet.vel = vel; - packet.vn = vn; - packet.ve = ve; - packet.vd = vd; - packet.cog = cog; - packet.fix_type = fix_type; - packet.satellites_visible = satellites_visible; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)&packet, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)&packet, MAVLINK_MSG_ID_HIL_GPS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_HIL_GPS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int32_t(buf, 8, lat); - _mav_put_int32_t(buf, 12, lon); - _mav_put_int32_t(buf, 16, alt); - _mav_put_uint16_t(buf, 20, eph); - _mav_put_uint16_t(buf, 22, epv); - _mav_put_uint16_t(buf, 24, vel); - _mav_put_int16_t(buf, 26, vn); - _mav_put_int16_t(buf, 28, ve); - _mav_put_int16_t(buf, 30, vd); - _mav_put_uint16_t(buf, 32, cog); - _mav_put_uint8_t(buf, 34, fix_type); - _mav_put_uint8_t(buf, 35, satellites_visible); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN); -#endif -#else - mavlink_hil_gps_t *packet = (mavlink_hil_gps_t *)msgbuf; - packet->time_usec = time_usec; - packet->lat = lat; - packet->lon = lon; - packet->alt = alt; - packet->eph = eph; - packet->epv = epv; - packet->vel = vel; - packet->vn = vn; - packet->ve = ve; - packet->vd = vd; - packet->cog = cog; - packet->fix_type = fix_type; - packet->satellites_visible = satellites_visible; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE HIL_GPS UNPACKING - - -/** - * @brief Get field time_usec from hil_gps message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_hil_gps_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field fix_type from hil_gps message - * - * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - */ -static inline uint8_t mavlink_msg_hil_gps_get_fix_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 34); -} - -/** - * @brief Get field lat from hil_gps message - * - * @return Latitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_hil_gps_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field lon from hil_gps message - * - * @return Longitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_hil_gps_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 12); -} - -/** - * @brief Get field alt from hil_gps message - * - * @return Altitude (WGS84), in meters * 1000 (positive for up) - */ -static inline int32_t mavlink_msg_hil_gps_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 16); -} - -/** - * @brief Get field eph from hil_gps message - * - * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - */ -static inline uint16_t mavlink_msg_hil_gps_get_eph(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 20); -} - -/** - * @brief Get field epv from hil_gps message - * - * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 - */ -static inline uint16_t mavlink_msg_hil_gps_get_epv(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 22); -} - -/** - * @brief Get field vel from hil_gps message - * - * @return GPS ground speed (m/s * 100). If unknown, set to: 65535 - */ -static inline uint16_t mavlink_msg_hil_gps_get_vel(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 24); -} - -/** - * @brief Get field vn from hil_gps message - * - * @return GPS velocity in cm/s in NORTH direction in earth-fixed NED frame - */ -static inline int16_t mavlink_msg_hil_gps_get_vn(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 26); -} - -/** - * @brief Get field ve from hil_gps message - * - * @return GPS velocity in cm/s in EAST direction in earth-fixed NED frame - */ -static inline int16_t mavlink_msg_hil_gps_get_ve(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 28); -} - -/** - * @brief Get field vd from hil_gps message - * - * @return GPS velocity in cm/s in DOWN direction in earth-fixed NED frame - */ -static inline int16_t mavlink_msg_hil_gps_get_vd(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 30); -} - -/** - * @brief Get field cog from hil_gps message - * - * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - */ -static inline uint16_t mavlink_msg_hil_gps_get_cog(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 32); -} - -/** - * @brief Get field satellites_visible from hil_gps message - * - * @return Number of satellites visible. If unknown, set to 255 - */ -static inline uint8_t mavlink_msg_hil_gps_get_satellites_visible(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 35); -} - -/** - * @brief Decode a hil_gps message into a struct - * - * @param msg The message to decode - * @param hil_gps C-struct to decode the message contents into - */ -static inline void mavlink_msg_hil_gps_decode(const mavlink_message_t* msg, mavlink_hil_gps_t* hil_gps) -{ -#if MAVLINK_NEED_BYTE_SWAP - hil_gps->time_usec = mavlink_msg_hil_gps_get_time_usec(msg); - hil_gps->lat = mavlink_msg_hil_gps_get_lat(msg); - hil_gps->lon = mavlink_msg_hil_gps_get_lon(msg); - hil_gps->alt = mavlink_msg_hil_gps_get_alt(msg); - hil_gps->eph = mavlink_msg_hil_gps_get_eph(msg); - hil_gps->epv = mavlink_msg_hil_gps_get_epv(msg); - hil_gps->vel = mavlink_msg_hil_gps_get_vel(msg); - hil_gps->vn = mavlink_msg_hil_gps_get_vn(msg); - hil_gps->ve = mavlink_msg_hil_gps_get_ve(msg); - hil_gps->vd = mavlink_msg_hil_gps_get_vd(msg); - hil_gps->cog = mavlink_msg_hil_gps_get_cog(msg); - hil_gps->fix_type = mavlink_msg_hil_gps_get_fix_type(msg); - hil_gps->satellites_visible = mavlink_msg_hil_gps_get_satellites_visible(msg); -#else - memcpy(hil_gps, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_GPS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h deleted file mode 100644 index eaadf2435..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h +++ /dev/null @@ -1,377 +0,0 @@ -// MESSAGE HIL_OPTICAL_FLOW PACKING - -#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW 114 - -typedef struct __mavlink_hil_optical_flow_t -{ - uint64_t time_usec; ///< Timestamp (UNIX) - float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated - float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated - float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - int16_t flow_x; ///< Flow in pixels in x-sensor direction - int16_t flow_y; ///< Flow in pixels in y-sensor direction - uint8_t sensor_id; ///< Sensor ID - uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality -} mavlink_hil_optical_flow_t; - -#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN 26 -#define MAVLINK_MSG_ID_114_LEN 26 - -#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC 119 -#define MAVLINK_MSG_ID_114_CRC 119 - - - -#define MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW { \ - "HIL_OPTICAL_FLOW", \ - 8, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_optical_flow_t, time_usec) }, \ - { "flow_comp_m_x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_optical_flow_t, flow_comp_m_x) }, \ - { "flow_comp_m_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_optical_flow_t, flow_comp_m_y) }, \ - { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_optical_flow_t, ground_distance) }, \ - { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_hil_optical_flow_t, flow_x) }, \ - { "flow_y", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_hil_optical_flow_t, flow_y) }, \ - { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_hil_optical_flow_t, sensor_id) }, \ - { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_hil_optical_flow_t, quality) }, \ - } \ -} - - -/** - * @brief Pack a hil_optical_flow message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (UNIX) - * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction - * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated - * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, flow_comp_m_x); - _mav_put_float(buf, 12, flow_comp_m_y); - _mav_put_float(buf, 16, ground_distance); - _mav_put_int16_t(buf, 20, flow_x); - _mav_put_int16_t(buf, 22, flow_y); - _mav_put_uint8_t(buf, 24, sensor_id); - _mav_put_uint8_t(buf, 25, quality); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); -#else - mavlink_hil_optical_flow_t packet; - packet.time_usec = time_usec; - packet.flow_comp_m_x = flow_comp_m_x; - packet.flow_comp_m_y = flow_comp_m_y; - packet.ground_distance = ground_distance; - packet.flow_x = flow_x; - packet.flow_y = flow_y; - packet.sensor_id = sensor_id; - packet.quality = quality; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_OPTICAL_FLOW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); -#endif -} - -/** - * @brief Pack a hil_optical_flow message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (UNIX) - * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction - * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated - * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,float flow_comp_m_x,float flow_comp_m_y,uint8_t quality,float ground_distance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, flow_comp_m_x); - _mav_put_float(buf, 12, flow_comp_m_y); - _mav_put_float(buf, 16, ground_distance); - _mav_put_int16_t(buf, 20, flow_x); - _mav_put_int16_t(buf, 22, flow_y); - _mav_put_uint8_t(buf, 24, sensor_id); - _mav_put_uint8_t(buf, 25, quality); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); -#else - mavlink_hil_optical_flow_t packet; - packet.time_usec = time_usec; - packet.flow_comp_m_x = flow_comp_m_x; - packet.flow_comp_m_y = flow_comp_m_y; - packet.ground_distance = ground_distance; - packet.flow_x = flow_x; - packet.flow_y = flow_y; - packet.sensor_id = sensor_id; - packet.quality = quality; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_OPTICAL_FLOW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); -#endif -} - -/** - * @brief Encode a hil_optical_flow struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param hil_optical_flow C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow) -{ - return mavlink_msg_hil_optical_flow_pack(system_id, component_id, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->flow_x, hil_optical_flow->flow_y, hil_optical_flow->flow_comp_m_x, hil_optical_flow->flow_comp_m_y, hil_optical_flow->quality, hil_optical_flow->ground_distance); -} - -/** - * @brief Encode a hil_optical_flow struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param hil_optical_flow C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow) -{ - return mavlink_msg_hil_optical_flow_pack_chan(system_id, component_id, chan, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->flow_x, hil_optical_flow->flow_y, hil_optical_flow->flow_comp_m_x, hil_optical_flow->flow_comp_m_y, hil_optical_flow->quality, hil_optical_flow->ground_distance); -} - -/** - * @brief Send a hil_optical_flow message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (UNIX) - * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction - * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated - * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_hil_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, flow_comp_m_x); - _mav_put_float(buf, 12, flow_comp_m_y); - _mav_put_float(buf, 16, ground_distance); - _mav_put_int16_t(buf, 20, flow_x); - _mav_put_int16_t(buf, 22, flow_y); - _mav_put_uint8_t(buf, 24, sensor_id); - _mav_put_uint8_t(buf, 25, quality); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); -#endif -#else - mavlink_hil_optical_flow_t packet; - packet.time_usec = time_usec; - packet.flow_comp_m_x = flow_comp_m_x; - packet.flow_comp_m_y = flow_comp_m_y; - packet.ground_distance = ground_distance; - packet.flow_x = flow_x; - packet.flow_y = flow_y; - packet.sensor_id = sensor_id; - packet.quality = quality; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_hil_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, flow_comp_m_x); - _mav_put_float(buf, 12, flow_comp_m_y); - _mav_put_float(buf, 16, ground_distance); - _mav_put_int16_t(buf, 20, flow_x); - _mav_put_int16_t(buf, 22, flow_y); - _mav_put_uint8_t(buf, 24, sensor_id); - _mav_put_uint8_t(buf, 25, quality); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); -#endif -#else - mavlink_hil_optical_flow_t *packet = (mavlink_hil_optical_flow_t *)msgbuf; - packet->time_usec = time_usec; - packet->flow_comp_m_x = flow_comp_m_x; - packet->flow_comp_m_y = flow_comp_m_y; - packet->ground_distance = ground_distance; - packet->flow_x = flow_x; - packet->flow_y = flow_y; - packet->sensor_id = sensor_id; - packet->quality = quality; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE HIL_OPTICAL_FLOW UNPACKING - - -/** - * @brief Get field time_usec from hil_optical_flow message - * - * @return Timestamp (UNIX) - */ -static inline uint64_t mavlink_msg_hil_optical_flow_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field sensor_id from hil_optical_flow message - * - * @return Sensor ID - */ -static inline uint8_t mavlink_msg_hil_optical_flow_get_sensor_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 24); -} - -/** - * @brief Get field flow_x from hil_optical_flow message - * - * @return Flow in pixels in x-sensor direction - */ -static inline int16_t mavlink_msg_hil_optical_flow_get_flow_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Get field flow_y from hil_optical_flow message - * - * @return Flow in pixels in y-sensor direction - */ -static inline int16_t mavlink_msg_hil_optical_flow_get_flow_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 22); -} - -/** - * @brief Get field flow_comp_m_x from hil_optical_flow message - * - * @return Flow in meters in x-sensor direction, angular-speed compensated - */ -static inline float mavlink_msg_hil_optical_flow_get_flow_comp_m_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field flow_comp_m_y from hil_optical_flow message - * - * @return Flow in meters in y-sensor direction, angular-speed compensated - */ -static inline float mavlink_msg_hil_optical_flow_get_flow_comp_m_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field quality from hil_optical_flow message - * - * @return Optical flow quality / confidence. 0: bad, 255: maximum quality - */ -static inline uint8_t mavlink_msg_hil_optical_flow_get_quality(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 25); -} - -/** - * @brief Get field ground_distance from hil_optical_flow message - * - * @return Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - */ -static inline float mavlink_msg_hil_optical_flow_get_ground_distance(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a hil_optical_flow message into a struct - * - * @param msg The message to decode - * @param hil_optical_flow C-struct to decode the message contents into - */ -static inline void mavlink_msg_hil_optical_flow_decode(const mavlink_message_t* msg, mavlink_hil_optical_flow_t* hil_optical_flow) -{ -#if MAVLINK_NEED_BYTE_SWAP - hil_optical_flow->time_usec = mavlink_msg_hil_optical_flow_get_time_usec(msg); - hil_optical_flow->flow_comp_m_x = mavlink_msg_hil_optical_flow_get_flow_comp_m_x(msg); - hil_optical_flow->flow_comp_m_y = mavlink_msg_hil_optical_flow_get_flow_comp_m_y(msg); - hil_optical_flow->ground_distance = mavlink_msg_hil_optical_flow_get_ground_distance(msg); - hil_optical_flow->flow_x = mavlink_msg_hil_optical_flow_get_flow_x(msg); - hil_optical_flow->flow_y = mavlink_msg_hil_optical_flow_get_flow_y(msg); - hil_optical_flow->sensor_id = mavlink_msg_hil_optical_flow_get_sensor_id(msg); - hil_optical_flow->quality = mavlink_msg_hil_optical_flow_get_quality(msg); -#else - memcpy(hil_optical_flow, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h deleted file mode 100644 index 227cd9d94..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h +++ /dev/null @@ -1,521 +0,0 @@ -// MESSAGE HIL_RC_INPUTS_RAW PACKING - -#define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW 92 - -typedef struct __mavlink_hil_rc_inputs_raw_t -{ - uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - uint16_t chan1_raw; ///< RC channel 1 value, in microseconds - uint16_t chan2_raw; ///< RC channel 2 value, in microseconds - uint16_t chan3_raw; ///< RC channel 3 value, in microseconds - uint16_t chan4_raw; ///< RC channel 4 value, in microseconds - uint16_t chan5_raw; ///< RC channel 5 value, in microseconds - uint16_t chan6_raw; ///< RC channel 6 value, in microseconds - uint16_t chan7_raw; ///< RC channel 7 value, in microseconds - uint16_t chan8_raw; ///< RC channel 8 value, in microseconds - uint16_t chan9_raw; ///< RC channel 9 value, in microseconds - uint16_t chan10_raw; ///< RC channel 10 value, in microseconds - uint16_t chan11_raw; ///< RC channel 11 value, in microseconds - uint16_t chan12_raw; ///< RC channel 12 value, in microseconds - uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% -} mavlink_hil_rc_inputs_raw_t; - -#define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN 33 -#define MAVLINK_MSG_ID_92_LEN 33 - -#define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC 54 -#define MAVLINK_MSG_ID_92_CRC 54 - - - -#define MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW { \ - "HIL_RC_INPUTS_RAW", \ - 14, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_rc_inputs_raw_t, time_usec) }, \ - { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_hil_rc_inputs_raw_t, chan1_raw) }, \ - { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_hil_rc_inputs_raw_t, chan2_raw) }, \ - { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_hil_rc_inputs_raw_t, chan3_raw) }, \ - { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_hil_rc_inputs_raw_t, chan4_raw) }, \ - { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_hil_rc_inputs_raw_t, chan5_raw) }, \ - { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_hil_rc_inputs_raw_t, chan6_raw) }, \ - { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_hil_rc_inputs_raw_t, chan7_raw) }, \ - { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_hil_rc_inputs_raw_t, chan8_raw) }, \ - { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_hil_rc_inputs_raw_t, chan9_raw) }, \ - { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_hil_rc_inputs_raw_t, chan10_raw) }, \ - { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_hil_rc_inputs_raw_t, chan11_raw) }, \ - { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_hil_rc_inputs_raw_t, chan12_raw) }, \ - { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_hil_rc_inputs_raw_t, rssi) }, \ - } \ -} - - -/** - * @brief Pack a hil_rc_inputs_raw message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds - * @param chan9_raw RC channel 9 value, in microseconds - * @param chan10_raw RC channel 10 value, in microseconds - * @param chan11_raw RC channel 11 value, in microseconds - * @param chan12_raw RC channel 12 value, in microseconds - * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_uint16_t(buf, 8, chan1_raw); - _mav_put_uint16_t(buf, 10, chan2_raw); - _mav_put_uint16_t(buf, 12, chan3_raw); - _mav_put_uint16_t(buf, 14, chan4_raw); - _mav_put_uint16_t(buf, 16, chan5_raw); - _mav_put_uint16_t(buf, 18, chan6_raw); - _mav_put_uint16_t(buf, 20, chan7_raw); - _mav_put_uint16_t(buf, 22, chan8_raw); - _mav_put_uint16_t(buf, 24, chan9_raw); - _mav_put_uint16_t(buf, 26, chan10_raw); - _mav_put_uint16_t(buf, 28, chan11_raw); - _mav_put_uint16_t(buf, 30, chan12_raw); - _mav_put_uint8_t(buf, 32, rssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); -#else - mavlink_hil_rc_inputs_raw_t packet; - packet.time_usec = time_usec; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - packet.chan9_raw = chan9_raw; - packet.chan10_raw = chan10_raw; - packet.chan11_raw = chan11_raw; - packet.chan12_raw = chan12_raw; - packet.rssi = rssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); -#endif -} - -/** - * @brief Pack a hil_rc_inputs_raw message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds - * @param chan9_raw RC channel 9 value, in microseconds - * @param chan10_raw RC channel 10 value, in microseconds - * @param chan11_raw RC channel 11 value, in microseconds - * @param chan12_raw RC channel 12 value, in microseconds - * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint16_t chan9_raw,uint16_t chan10_raw,uint16_t chan11_raw,uint16_t chan12_raw,uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_uint16_t(buf, 8, chan1_raw); - _mav_put_uint16_t(buf, 10, chan2_raw); - _mav_put_uint16_t(buf, 12, chan3_raw); - _mav_put_uint16_t(buf, 14, chan4_raw); - _mav_put_uint16_t(buf, 16, chan5_raw); - _mav_put_uint16_t(buf, 18, chan6_raw); - _mav_put_uint16_t(buf, 20, chan7_raw); - _mav_put_uint16_t(buf, 22, chan8_raw); - _mav_put_uint16_t(buf, 24, chan9_raw); - _mav_put_uint16_t(buf, 26, chan10_raw); - _mav_put_uint16_t(buf, 28, chan11_raw); - _mav_put_uint16_t(buf, 30, chan12_raw); - _mav_put_uint8_t(buf, 32, rssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); -#else - mavlink_hil_rc_inputs_raw_t packet; - packet.time_usec = time_usec; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - packet.chan9_raw = chan9_raw; - packet.chan10_raw = chan10_raw; - packet.chan11_raw = chan11_raw; - packet.chan12_raw = chan12_raw; - packet.rssi = rssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); -#endif -} - -/** - * @brief Encode a hil_rc_inputs_raw struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param hil_rc_inputs_raw C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw) -{ - return mavlink_msg_hil_rc_inputs_raw_pack(system_id, component_id, msg, hil_rc_inputs_raw->time_usec, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi); -} - -/** - * @brief Encode a hil_rc_inputs_raw struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param hil_rc_inputs_raw C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw) -{ - return mavlink_msg_hil_rc_inputs_raw_pack_chan(system_id, component_id, chan, msg, hil_rc_inputs_raw->time_usec, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi); -} - -/** - * @brief Send a hil_rc_inputs_raw message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds - * @param chan9_raw RC channel 9 value, in microseconds - * @param chan10_raw RC channel 10 value, in microseconds - * @param chan11_raw RC channel 11 value, in microseconds - * @param chan12_raw RC channel 12 value, in microseconds - * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_hil_rc_inputs_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_uint16_t(buf, 8, chan1_raw); - _mav_put_uint16_t(buf, 10, chan2_raw); - _mav_put_uint16_t(buf, 12, chan3_raw); - _mav_put_uint16_t(buf, 14, chan4_raw); - _mav_put_uint16_t(buf, 16, chan5_raw); - _mav_put_uint16_t(buf, 18, chan6_raw); - _mav_put_uint16_t(buf, 20, chan7_raw); - _mav_put_uint16_t(buf, 22, chan8_raw); - _mav_put_uint16_t(buf, 24, chan9_raw); - _mav_put_uint16_t(buf, 26, chan10_raw); - _mav_put_uint16_t(buf, 28, chan11_raw); - _mav_put_uint16_t(buf, 30, chan12_raw); - _mav_put_uint8_t(buf, 32, rssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); -#endif -#else - mavlink_hil_rc_inputs_raw_t packet; - packet.time_usec = time_usec; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - packet.chan9_raw = chan9_raw; - packet.chan10_raw = chan10_raw; - packet.chan11_raw = chan11_raw; - packet.chan12_raw = chan12_raw; - packet.rssi = rssi; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)&packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)&packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_hil_rc_inputs_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_uint16_t(buf, 8, chan1_raw); - _mav_put_uint16_t(buf, 10, chan2_raw); - _mav_put_uint16_t(buf, 12, chan3_raw); - _mav_put_uint16_t(buf, 14, chan4_raw); - _mav_put_uint16_t(buf, 16, chan5_raw); - _mav_put_uint16_t(buf, 18, chan6_raw); - _mav_put_uint16_t(buf, 20, chan7_raw); - _mav_put_uint16_t(buf, 22, chan8_raw); - _mav_put_uint16_t(buf, 24, chan9_raw); - _mav_put_uint16_t(buf, 26, chan10_raw); - _mav_put_uint16_t(buf, 28, chan11_raw); - _mav_put_uint16_t(buf, 30, chan12_raw); - _mav_put_uint8_t(buf, 32, rssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); -#endif -#else - mavlink_hil_rc_inputs_raw_t *packet = (mavlink_hil_rc_inputs_raw_t *)msgbuf; - packet->time_usec = time_usec; - packet->chan1_raw = chan1_raw; - packet->chan2_raw = chan2_raw; - packet->chan3_raw = chan3_raw; - packet->chan4_raw = chan4_raw; - packet->chan5_raw = chan5_raw; - packet->chan6_raw = chan6_raw; - packet->chan7_raw = chan7_raw; - packet->chan8_raw = chan8_raw; - packet->chan9_raw = chan9_raw; - packet->chan10_raw = chan10_raw; - packet->chan11_raw = chan11_raw; - packet->chan12_raw = chan12_raw; - packet->rssi = rssi; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE HIL_RC_INPUTS_RAW UNPACKING - - -/** - * @brief Get field time_usec from hil_rc_inputs_raw message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_hil_rc_inputs_raw_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field chan1_raw from hil_rc_inputs_raw message - * - * @return RC channel 1 value, in microseconds - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan1_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field chan2_raw from hil_rc_inputs_raw message - * - * @return RC channel 2 value, in microseconds - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan2_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Get field chan3_raw from hil_rc_inputs_raw message - * - * @return RC channel 3 value, in microseconds - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan3_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field chan4_raw from hil_rc_inputs_raw message - * - * @return RC channel 4 value, in microseconds - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan4_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 14); -} - -/** - * @brief Get field chan5_raw from hil_rc_inputs_raw message - * - * @return RC channel 5 value, in microseconds - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan5_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field chan6_raw from hil_rc_inputs_raw message - * - * @return RC channel 6 value, in microseconds - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan6_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 18); -} - -/** - * @brief Get field chan7_raw from hil_rc_inputs_raw message - * - * @return RC channel 7 value, in microseconds - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan7_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 20); -} - -/** - * @brief Get field chan8_raw from hil_rc_inputs_raw message - * - * @return RC channel 8 value, in microseconds - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan8_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 22); -} - -/** - * @brief Get field chan9_raw from hil_rc_inputs_raw message - * - * @return RC channel 9 value, in microseconds - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan9_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 24); -} - -/** - * @brief Get field chan10_raw from hil_rc_inputs_raw message - * - * @return RC channel 10 value, in microseconds - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan10_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 26); -} - -/** - * @brief Get field chan11_raw from hil_rc_inputs_raw message - * - * @return RC channel 11 value, in microseconds - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan11_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 28); -} - -/** - * @brief Get field chan12_raw from hil_rc_inputs_raw message - * - * @return RC channel 12 value, in microseconds - */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 30); -} - -/** - * @brief Get field rssi from hil_rc_inputs_raw message - * - * @return Receive signal strength indicator, 0: 0%, 255: 100% - */ -static inline uint8_t mavlink_msg_hil_rc_inputs_raw_get_rssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Decode a hil_rc_inputs_raw message into a struct - * - * @param msg The message to decode - * @param hil_rc_inputs_raw C-struct to decode the message contents into - */ -static inline void mavlink_msg_hil_rc_inputs_raw_decode(const mavlink_message_t* msg, mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP - hil_rc_inputs_raw->time_usec = mavlink_msg_hil_rc_inputs_raw_get_time_usec(msg); - hil_rc_inputs_raw->chan1_raw = mavlink_msg_hil_rc_inputs_raw_get_chan1_raw(msg); - hil_rc_inputs_raw->chan2_raw = mavlink_msg_hil_rc_inputs_raw_get_chan2_raw(msg); - hil_rc_inputs_raw->chan3_raw = mavlink_msg_hil_rc_inputs_raw_get_chan3_raw(msg); - hil_rc_inputs_raw->chan4_raw = mavlink_msg_hil_rc_inputs_raw_get_chan4_raw(msg); - hil_rc_inputs_raw->chan5_raw = mavlink_msg_hil_rc_inputs_raw_get_chan5_raw(msg); - hil_rc_inputs_raw->chan6_raw = mavlink_msg_hil_rc_inputs_raw_get_chan6_raw(msg); - hil_rc_inputs_raw->chan7_raw = mavlink_msg_hil_rc_inputs_raw_get_chan7_raw(msg); - hil_rc_inputs_raw->chan8_raw = mavlink_msg_hil_rc_inputs_raw_get_chan8_raw(msg); - hil_rc_inputs_raw->chan9_raw = mavlink_msg_hil_rc_inputs_raw_get_chan9_raw(msg); - hil_rc_inputs_raw->chan10_raw = mavlink_msg_hil_rc_inputs_raw_get_chan10_raw(msg); - hil_rc_inputs_raw->chan11_raw = mavlink_msg_hil_rc_inputs_raw_get_chan11_raw(msg); - hil_rc_inputs_raw->chan12_raw = mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(msg); - hil_rc_inputs_raw->rssi = mavlink_msg_hil_rc_inputs_raw_get_rssi(msg); -#else - memcpy(hil_rc_inputs_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h deleted file mode 100644 index 8672f8b8a..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h +++ /dev/null @@ -1,545 +0,0 @@ -// MESSAGE HIL_SENSOR PACKING - -#define MAVLINK_MSG_ID_HIL_SENSOR 107 - -typedef struct __mavlink_hil_sensor_t -{ - uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) - float xacc; ///< X acceleration (m/s^2) - float yacc; ///< Y acceleration (m/s^2) - float zacc; ///< Z acceleration (m/s^2) - float xgyro; ///< Angular speed around X axis in body frame (rad / sec) - float ygyro; ///< Angular speed around Y axis in body frame (rad / sec) - float zgyro; ///< Angular speed around Z axis in body frame (rad / sec) - float xmag; ///< X Magnetic field (Gauss) - float ymag; ///< Y Magnetic field (Gauss) - float zmag; ///< Z Magnetic field (Gauss) - float abs_pressure; ///< Absolute pressure in millibar - float diff_pressure; ///< Differential pressure (airspeed) in millibar - float pressure_alt; ///< Altitude calculated from pressure - float temperature; ///< Temperature in degrees celsius - uint32_t fields_updated; ///< Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature -} mavlink_hil_sensor_t; - -#define MAVLINK_MSG_ID_HIL_SENSOR_LEN 64 -#define MAVLINK_MSG_ID_107_LEN 64 - -#define MAVLINK_MSG_ID_HIL_SENSOR_CRC 108 -#define MAVLINK_MSG_ID_107_CRC 108 - - - -#define MAVLINK_MESSAGE_INFO_HIL_SENSOR { \ - "HIL_SENSOR", \ - 15, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_sensor_t, time_usec) }, \ - { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_sensor_t, xacc) }, \ - { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_sensor_t, yacc) }, \ - { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_sensor_t, zacc) }, \ - { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_sensor_t, xgyro) }, \ - { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_sensor_t, ygyro) }, \ - { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_sensor_t, zgyro) }, \ - { "xmag", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_hil_sensor_t, xmag) }, \ - { "ymag", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_hil_sensor_t, ymag) }, \ - { "zmag", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_hil_sensor_t, zmag) }, \ - { "abs_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_hil_sensor_t, abs_pressure) }, \ - { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_hil_sensor_t, diff_pressure) }, \ - { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_hil_sensor_t, pressure_alt) }, \ - { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_hil_sensor_t, temperature) }, \ - { "fields_updated", NULL, MAVLINK_TYPE_UINT32_T, 0, 60, offsetof(mavlink_hil_sensor_t, fields_updated) }, \ - } \ -} - - -/** - * @brief Pack a hil_sensor message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param xacc X acceleration (m/s^2) - * @param yacc Y acceleration (m/s^2) - * @param zacc Z acceleration (m/s^2) - * @param xgyro Angular speed around X axis in body frame (rad / sec) - * @param ygyro Angular speed around Y axis in body frame (rad / sec) - * @param zgyro Angular speed around Z axis in body frame (rad / sec) - * @param xmag X Magnetic field (Gauss) - * @param ymag Y Magnetic field (Gauss) - * @param zmag Z Magnetic field (Gauss) - * @param abs_pressure Absolute pressure in millibar - * @param diff_pressure Differential pressure (airspeed) in millibar - * @param pressure_alt Altitude calculated from pressure - * @param temperature Temperature in degrees celsius - * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, xacc); - _mav_put_float(buf, 12, yacc); - _mav_put_float(buf, 16, zacc); - _mav_put_float(buf, 20, xgyro); - _mav_put_float(buf, 24, ygyro); - _mav_put_float(buf, 28, zgyro); - _mav_put_float(buf, 32, xmag); - _mav_put_float(buf, 36, ymag); - _mav_put_float(buf, 40, zmag); - _mav_put_float(buf, 44, abs_pressure); - _mav_put_float(buf, 48, diff_pressure); - _mav_put_float(buf, 52, pressure_alt); - _mav_put_float(buf, 56, temperature); - _mav_put_uint32_t(buf, 60, fields_updated); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); -#else - mavlink_hil_sensor_t packet; - packet.time_usec = time_usec; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - packet.abs_pressure = abs_pressure; - packet.diff_pressure = diff_pressure; - packet.pressure_alt = pressure_alt; - packet.temperature = temperature; - packet.fields_updated = fields_updated; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_SENSOR; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_SENSOR_LEN); -#endif -} - -/** - * @brief Pack a hil_sensor message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param xacc X acceleration (m/s^2) - * @param yacc Y acceleration (m/s^2) - * @param zacc Z acceleration (m/s^2) - * @param xgyro Angular speed around X axis in body frame (rad / sec) - * @param ygyro Angular speed around Y axis in body frame (rad / sec) - * @param zgyro Angular speed around Z axis in body frame (rad / sec) - * @param xmag X Magnetic field (Gauss) - * @param ymag Y Magnetic field (Gauss) - * @param zmag Z Magnetic field (Gauss) - * @param abs_pressure Absolute pressure in millibar - * @param diff_pressure Differential pressure (airspeed) in millibar - * @param pressure_alt Altitude calculated from pressure - * @param temperature Temperature in degrees celsius - * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint32_t fields_updated) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, xacc); - _mav_put_float(buf, 12, yacc); - _mav_put_float(buf, 16, zacc); - _mav_put_float(buf, 20, xgyro); - _mav_put_float(buf, 24, ygyro); - _mav_put_float(buf, 28, zgyro); - _mav_put_float(buf, 32, xmag); - _mav_put_float(buf, 36, ymag); - _mav_put_float(buf, 40, zmag); - _mav_put_float(buf, 44, abs_pressure); - _mav_put_float(buf, 48, diff_pressure); - _mav_put_float(buf, 52, pressure_alt); - _mav_put_float(buf, 56, temperature); - _mav_put_uint32_t(buf, 60, fields_updated); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); -#else - mavlink_hil_sensor_t packet; - packet.time_usec = time_usec; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - packet.abs_pressure = abs_pressure; - packet.diff_pressure = diff_pressure; - packet.pressure_alt = pressure_alt; - packet.temperature = temperature; - packet.fields_updated = fields_updated; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_SENSOR; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_SENSOR_LEN); -#endif -} - -/** - * @brief Encode a hil_sensor struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param hil_sensor C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) -{ - return mavlink_msg_hil_sensor_pack(system_id, component_id, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); -} - -/** - * @brief Encode a hil_sensor struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param hil_sensor C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) -{ - return mavlink_msg_hil_sensor_pack_chan(system_id, component_id, chan, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); -} - -/** - * @brief Send a hil_sensor message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param xacc X acceleration (m/s^2) - * @param yacc Y acceleration (m/s^2) - * @param zacc Z acceleration (m/s^2) - * @param xgyro Angular speed around X axis in body frame (rad / sec) - * @param ygyro Angular speed around Y axis in body frame (rad / sec) - * @param zgyro Angular speed around Z axis in body frame (rad / sec) - * @param xmag X Magnetic field (Gauss) - * @param ymag Y Magnetic field (Gauss) - * @param zmag Z Magnetic field (Gauss) - * @param abs_pressure Absolute pressure in millibar - * @param diff_pressure Differential pressure (airspeed) in millibar - * @param pressure_alt Altitude calculated from pressure - * @param temperature Temperature in degrees celsius - * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, xacc); - _mav_put_float(buf, 12, yacc); - _mav_put_float(buf, 16, zacc); - _mav_put_float(buf, 20, xgyro); - _mav_put_float(buf, 24, ygyro); - _mav_put_float(buf, 28, zgyro); - _mav_put_float(buf, 32, xmag); - _mav_put_float(buf, 36, ymag); - _mav_put_float(buf, 40, zmag); - _mav_put_float(buf, 44, abs_pressure); - _mav_put_float(buf, 48, diff_pressure); - _mav_put_float(buf, 52, pressure_alt); - _mav_put_float(buf, 56, temperature); - _mav_put_uint32_t(buf, 60, fields_updated); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); -#endif -#else - mavlink_hil_sensor_t packet; - packet.time_usec = time_usec; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - packet.abs_pressure = abs_pressure; - packet.diff_pressure = diff_pressure; - packet.pressure_alt = pressure_alt; - packet.temperature = temperature; - packet.fields_updated = fields_updated; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_HIL_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, xacc); - _mav_put_float(buf, 12, yacc); - _mav_put_float(buf, 16, zacc); - _mav_put_float(buf, 20, xgyro); - _mav_put_float(buf, 24, ygyro); - _mav_put_float(buf, 28, zgyro); - _mav_put_float(buf, 32, xmag); - _mav_put_float(buf, 36, ymag); - _mav_put_float(buf, 40, zmag); - _mav_put_float(buf, 44, abs_pressure); - _mav_put_float(buf, 48, diff_pressure); - _mav_put_float(buf, 52, pressure_alt); - _mav_put_float(buf, 56, temperature); - _mav_put_uint32_t(buf, 60, fields_updated); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); -#endif -#else - mavlink_hil_sensor_t *packet = (mavlink_hil_sensor_t *)msgbuf; - packet->time_usec = time_usec; - packet->xacc = xacc; - packet->yacc = yacc; - packet->zacc = zacc; - packet->xgyro = xgyro; - packet->ygyro = ygyro; - packet->zgyro = zgyro; - packet->xmag = xmag; - packet->ymag = ymag; - packet->zmag = zmag; - packet->abs_pressure = abs_pressure; - packet->diff_pressure = diff_pressure; - packet->pressure_alt = pressure_alt; - packet->temperature = temperature; - packet->fields_updated = fields_updated; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE HIL_SENSOR UNPACKING - - -/** - * @brief Get field time_usec from hil_sensor message - * - * @return Timestamp (microseconds, synced to UNIX time or since system boot) - */ -static inline uint64_t mavlink_msg_hil_sensor_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field xacc from hil_sensor message - * - * @return X acceleration (m/s^2) - */ -static inline float mavlink_msg_hil_sensor_get_xacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yacc from hil_sensor message - * - * @return Y acceleration (m/s^2) - */ -static inline float mavlink_msg_hil_sensor_get_yacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field zacc from hil_sensor message - * - * @return Z acceleration (m/s^2) - */ -static inline float mavlink_msg_hil_sensor_get_zacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field xgyro from hil_sensor message - * - * @return Angular speed around X axis in body frame (rad / sec) - */ -static inline float mavlink_msg_hil_sensor_get_xgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field ygyro from hil_sensor message - * - * @return Angular speed around Y axis in body frame (rad / sec) - */ -static inline float mavlink_msg_hil_sensor_get_ygyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field zgyro from hil_sensor message - * - * @return Angular speed around Z axis in body frame (rad / sec) - */ -static inline float mavlink_msg_hil_sensor_get_zgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field xmag from hil_sensor message - * - * @return X Magnetic field (Gauss) - */ -static inline float mavlink_msg_hil_sensor_get_xmag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field ymag from hil_sensor message - * - * @return Y Magnetic field (Gauss) - */ -static inline float mavlink_msg_hil_sensor_get_ymag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 36); -} - -/** - * @brief Get field zmag from hil_sensor message - * - * @return Z Magnetic field (Gauss) - */ -static inline float mavlink_msg_hil_sensor_get_zmag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 40); -} - -/** - * @brief Get field abs_pressure from hil_sensor message - * - * @return Absolute pressure in millibar - */ -static inline float mavlink_msg_hil_sensor_get_abs_pressure(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 44); -} - -/** - * @brief Get field diff_pressure from hil_sensor message - * - * @return Differential pressure (airspeed) in millibar - */ -static inline float mavlink_msg_hil_sensor_get_diff_pressure(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 48); -} - -/** - * @brief Get field pressure_alt from hil_sensor message - * - * @return Altitude calculated from pressure - */ -static inline float mavlink_msg_hil_sensor_get_pressure_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 52); -} - -/** - * @brief Get field temperature from hil_sensor message - * - * @return Temperature in degrees celsius - */ -static inline float mavlink_msg_hil_sensor_get_temperature(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 56); -} - -/** - * @brief Get field fields_updated from hil_sensor message - * - * @return Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature - */ -static inline uint32_t mavlink_msg_hil_sensor_get_fields_updated(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 60); -} - -/** - * @brief Decode a hil_sensor message into a struct - * - * @param msg The message to decode - * @param hil_sensor C-struct to decode the message contents into - */ -static inline void mavlink_msg_hil_sensor_decode(const mavlink_message_t* msg, mavlink_hil_sensor_t* hil_sensor) -{ -#if MAVLINK_NEED_BYTE_SWAP - hil_sensor->time_usec = mavlink_msg_hil_sensor_get_time_usec(msg); - hil_sensor->xacc = mavlink_msg_hil_sensor_get_xacc(msg); - hil_sensor->yacc = mavlink_msg_hil_sensor_get_yacc(msg); - hil_sensor->zacc = mavlink_msg_hil_sensor_get_zacc(msg); - hil_sensor->xgyro = mavlink_msg_hil_sensor_get_xgyro(msg); - hil_sensor->ygyro = mavlink_msg_hil_sensor_get_ygyro(msg); - hil_sensor->zgyro = mavlink_msg_hil_sensor_get_zgyro(msg); - hil_sensor->xmag = mavlink_msg_hil_sensor_get_xmag(msg); - hil_sensor->ymag = mavlink_msg_hil_sensor_get_ymag(msg); - hil_sensor->zmag = mavlink_msg_hil_sensor_get_zmag(msg); - hil_sensor->abs_pressure = mavlink_msg_hil_sensor_get_abs_pressure(msg); - hil_sensor->diff_pressure = mavlink_msg_hil_sensor_get_diff_pressure(msg); - hil_sensor->pressure_alt = mavlink_msg_hil_sensor_get_pressure_alt(msg); - hil_sensor->temperature = mavlink_msg_hil_sensor_get_temperature(msg); - hil_sensor->fields_updated = mavlink_msg_hil_sensor_get_fields_updated(msg); -#else - memcpy(hil_sensor, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_SENSOR_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h deleted file mode 100644 index 923ed60b9..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h +++ /dev/null @@ -1,569 +0,0 @@ -// MESSAGE HIL_STATE PACKING - -#define MAVLINK_MSG_ID_HIL_STATE 90 - -typedef struct __mavlink_hil_state_t -{ - uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float roll; ///< Roll angle (rad) - float pitch; ///< Pitch angle (rad) - float yaw; ///< Yaw angle (rad) - float rollspeed; ///< Body frame roll / phi angular speed (rad/s) - float pitchspeed; ///< Body frame pitch / theta angular speed (rad/s) - float yawspeed; ///< Body frame yaw / psi angular speed (rad/s) - int32_t lat; ///< Latitude, expressed as * 1E7 - int32_t lon; ///< Longitude, expressed as * 1E7 - int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) - int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 - int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 - int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 - int16_t xacc; ///< X acceleration (mg) - int16_t yacc; ///< Y acceleration (mg) - int16_t zacc; ///< Z acceleration (mg) -} mavlink_hil_state_t; - -#define MAVLINK_MSG_ID_HIL_STATE_LEN 56 -#define MAVLINK_MSG_ID_90_LEN 56 - -#define MAVLINK_MSG_ID_HIL_STATE_CRC 183 -#define MAVLINK_MSG_ID_90_CRC 183 - - - -#define MAVLINK_MESSAGE_INFO_HIL_STATE { \ - "HIL_STATE", \ - 16, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_state_t, time_usec) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_state_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_state_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_state_t, yaw) }, \ - { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_state_t, rollspeed) }, \ - { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_state_t, pitchspeed) }, \ - { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_state_t, yawspeed) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_hil_state_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_hil_state_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_hil_state_t, alt) }, \ - { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 44, offsetof(mavlink_hil_state_t, vx) }, \ - { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_hil_state_t, vy) }, \ - { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_hil_state_t, vz) }, \ - { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_hil_state_t, xacc) }, \ - { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_hil_state_t, yacc) }, \ - { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_hil_state_t, zacc) }, \ - } \ -} - - -/** - * @brief Pack a hil_state message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param rollspeed Body frame roll / phi angular speed (rad/s) - * @param pitchspeed Body frame pitch / theta angular speed (rad/s) - * @param yawspeed Body frame yaw / psi angular speed (rad/s) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) - * @param vx Ground X Speed (Latitude), expressed as m/s * 100 - * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_STATE_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, roll); - _mav_put_float(buf, 12, pitch); - _mav_put_float(buf, 16, yaw); - _mav_put_float(buf, 20, rollspeed); - _mav_put_float(buf, 24, pitchspeed); - _mav_put_float(buf, 28, yawspeed); - _mav_put_int32_t(buf, 32, lat); - _mav_put_int32_t(buf, 36, lon); - _mav_put_int32_t(buf, 40, alt); - _mav_put_int16_t(buf, 44, vx); - _mav_put_int16_t(buf, 46, vy); - _mav_put_int16_t(buf, 48, vz); - _mav_put_int16_t(buf, 50, xacc); - _mav_put_int16_t(buf, 52, yacc); - _mav_put_int16_t(buf, 54, zacc); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_LEN); -#else - mavlink_hil_state_t packet; - packet.time_usec = time_usec; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_STATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_LEN); -#endif -} - -/** - * @brief Pack a hil_state message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param rollspeed Body frame roll / phi angular speed (rad/s) - * @param pitchspeed Body frame pitch / theta angular speed (rad/s) - * @param yawspeed Body frame yaw / psi angular speed (rad/s) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) - * @param vx Ground X Speed (Latitude), expressed as m/s * 100 - * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,float roll,float pitch,float yaw,float rollspeed,float pitchspeed,float yawspeed,int32_t lat,int32_t lon,int32_t alt,int16_t vx,int16_t vy,int16_t vz,int16_t xacc,int16_t yacc,int16_t zacc) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_STATE_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, roll); - _mav_put_float(buf, 12, pitch); - _mav_put_float(buf, 16, yaw); - _mav_put_float(buf, 20, rollspeed); - _mav_put_float(buf, 24, pitchspeed); - _mav_put_float(buf, 28, yawspeed); - _mav_put_int32_t(buf, 32, lat); - _mav_put_int32_t(buf, 36, lon); - _mav_put_int32_t(buf, 40, alt); - _mav_put_int16_t(buf, 44, vx); - _mav_put_int16_t(buf, 46, vy); - _mav_put_int16_t(buf, 48, vz); - _mav_put_int16_t(buf, 50, xacc); - _mav_put_int16_t(buf, 52, yacc); - _mav_put_int16_t(buf, 54, zacc); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_LEN); -#else - mavlink_hil_state_t packet; - packet.time_usec = time_usec; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_STATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_LEN); -#endif -} - -/** - * @brief Encode a hil_state struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param hil_state C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state) -{ - return mavlink_msg_hil_state_pack(system_id, component_id, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); -} - -/** - * @brief Encode a hil_state struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param hil_state C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state) -{ - return mavlink_msg_hil_state_pack_chan(system_id, component_id, chan, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); -} - -/** - * @brief Send a hil_state message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param rollspeed Body frame roll / phi angular speed (rad/s) - * @param pitchspeed Body frame pitch / theta angular speed (rad/s) - * @param yawspeed Body frame yaw / psi angular speed (rad/s) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) - * @param vx Ground X Speed (Latitude), expressed as m/s * 100 - * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_STATE_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, roll); - _mav_put_float(buf, 12, pitch); - _mav_put_float(buf, 16, yaw); - _mav_put_float(buf, 20, rollspeed); - _mav_put_float(buf, 24, pitchspeed); - _mav_put_float(buf, 28, yawspeed); - _mav_put_int32_t(buf, 32, lat); - _mav_put_int32_t(buf, 36, lon); - _mav_put_int32_t(buf, 40, alt); - _mav_put_int16_t(buf, 44, vx); - _mav_put_int16_t(buf, 46, vy); - _mav_put_int16_t(buf, 48, vz); - _mav_put_int16_t(buf, 50, xacc); - _mav_put_int16_t(buf, 52, yacc); - _mav_put_int16_t(buf, 54, zacc); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN); -#endif -#else - mavlink_hil_state_t packet; - packet.time_usec = time_usec; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)&packet, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)&packet, MAVLINK_MSG_ID_HIL_STATE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_HIL_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_hil_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, roll); - _mav_put_float(buf, 12, pitch); - _mav_put_float(buf, 16, yaw); - _mav_put_float(buf, 20, rollspeed); - _mav_put_float(buf, 24, pitchspeed); - _mav_put_float(buf, 28, yawspeed); - _mav_put_int32_t(buf, 32, lat); - _mav_put_int32_t(buf, 36, lon); - _mav_put_int32_t(buf, 40, alt); - _mav_put_int16_t(buf, 44, vx); - _mav_put_int16_t(buf, 46, vy); - _mav_put_int16_t(buf, 48, vz); - _mav_put_int16_t(buf, 50, xacc); - _mav_put_int16_t(buf, 52, yacc); - _mav_put_int16_t(buf, 54, zacc); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN); -#endif -#else - mavlink_hil_state_t *packet = (mavlink_hil_state_t *)msgbuf; - packet->time_usec = time_usec; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - packet->rollspeed = rollspeed; - packet->pitchspeed = pitchspeed; - packet->yawspeed = yawspeed; - packet->lat = lat; - packet->lon = lon; - packet->alt = alt; - packet->vx = vx; - packet->vy = vy; - packet->vz = vz; - packet->xacc = xacc; - packet->yacc = yacc; - packet->zacc = zacc; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE HIL_STATE UNPACKING - - -/** - * @brief Get field time_usec from hil_state message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_hil_state_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field roll from hil_state message - * - * @return Roll angle (rad) - */ -static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field pitch from hil_state message - * - * @return Pitch angle (rad) - */ -static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field yaw from hil_state message - * - * @return Yaw angle (rad) - */ -static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field rollspeed from hil_state message - * - * @return Body frame roll / phi angular speed (rad/s) - */ -static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field pitchspeed from hil_state message - * - * @return Body frame pitch / theta angular speed (rad/s) - */ -static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field yawspeed from hil_state message - * - * @return Body frame yaw / psi angular speed (rad/s) - */ -static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field lat from hil_state message - * - * @return Latitude, expressed as * 1E7 - */ -static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 32); -} - -/** - * @brief Get field lon from hil_state message - * - * @return Longitude, expressed as * 1E7 - */ -static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 36); -} - -/** - * @brief Get field alt from hil_state message - * - * @return Altitude in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 40); -} - -/** - * @brief Get field vx from hil_state message - * - * @return Ground X Speed (Latitude), expressed as m/s * 100 - */ -static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 44); -} - -/** - * @brief Get field vy from hil_state message - * - * @return Ground Y Speed (Longitude), expressed as m/s * 100 - */ -static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 46); -} - -/** - * @brief Get field vz from hil_state message - * - * @return Ground Z Speed (Altitude), expressed as m/s * 100 - */ -static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 48); -} - -/** - * @brief Get field xacc from hil_state message - * - * @return X acceleration (mg) - */ -static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 50); -} - -/** - * @brief Get field yacc from hil_state message - * - * @return Y acceleration (mg) - */ -static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 52); -} - -/** - * @brief Get field zacc from hil_state message - * - * @return Z acceleration (mg) - */ -static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 54); -} - -/** - * @brief Decode a hil_state message into a struct - * - * @param msg The message to decode - * @param hil_state C-struct to decode the message contents into - */ -static inline void mavlink_msg_hil_state_decode(const mavlink_message_t* msg, mavlink_hil_state_t* hil_state) -{ -#if MAVLINK_NEED_BYTE_SWAP - hil_state->time_usec = mavlink_msg_hil_state_get_time_usec(msg); - hil_state->roll = mavlink_msg_hil_state_get_roll(msg); - hil_state->pitch = mavlink_msg_hil_state_get_pitch(msg); - hil_state->yaw = mavlink_msg_hil_state_get_yaw(msg); - hil_state->rollspeed = mavlink_msg_hil_state_get_rollspeed(msg); - hil_state->pitchspeed = mavlink_msg_hil_state_get_pitchspeed(msg); - hil_state->yawspeed = mavlink_msg_hil_state_get_yawspeed(msg); - hil_state->lat = mavlink_msg_hil_state_get_lat(msg); - hil_state->lon = mavlink_msg_hil_state_get_lon(msg); - hil_state->alt = mavlink_msg_hil_state_get_alt(msg); - hil_state->vx = mavlink_msg_hil_state_get_vx(msg); - hil_state->vy = mavlink_msg_hil_state_get_vy(msg); - hil_state->vz = mavlink_msg_hil_state_get_vz(msg); - hil_state->xacc = mavlink_msg_hil_state_get_xacc(msg); - hil_state->yacc = mavlink_msg_hil_state_get_yacc(msg); - hil_state->zacc = mavlink_msg_hil_state_get_zacc(msg); -#else - memcpy(hil_state, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_STATE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h deleted file mode 100644 index 1a028bdf9..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h +++ /dev/null @@ -1,561 +0,0 @@ -// MESSAGE HIL_STATE_QUATERNION PACKING - -#define MAVLINK_MSG_ID_HIL_STATE_QUATERNION 115 - -typedef struct __mavlink_hil_state_quaternion_t -{ - uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float attitude_quaternion[4]; ///< Vehicle attitude expressed as normalized quaternion - float rollspeed; ///< Body frame roll / phi angular speed (rad/s) - float pitchspeed; ///< Body frame pitch / theta angular speed (rad/s) - float yawspeed; ///< Body frame yaw / psi angular speed (rad/s) - int32_t lat; ///< Latitude, expressed as * 1E7 - int32_t lon; ///< Longitude, expressed as * 1E7 - int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) - int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 - int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 - int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 - uint16_t ind_airspeed; ///< Indicated airspeed, expressed as m/s * 100 - uint16_t true_airspeed; ///< True airspeed, expressed as m/s * 100 - int16_t xacc; ///< X acceleration (mg) - int16_t yacc; ///< Y acceleration (mg) - int16_t zacc; ///< Z acceleration (mg) -} mavlink_hil_state_quaternion_t; - -#define MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN 64 -#define MAVLINK_MSG_ID_115_LEN 64 - -#define MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC 4 -#define MAVLINK_MSG_ID_115_CRC 4 - -#define MAVLINK_MSG_HIL_STATE_QUATERNION_FIELD_ATTITUDE_QUATERNION_LEN 4 - -#define MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION { \ - "HIL_STATE_QUATERNION", \ - 16, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_state_quaternion_t, time_usec) }, \ - { "attitude_quaternion", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_hil_state_quaternion_t, attitude_quaternion) }, \ - { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_state_quaternion_t, rollspeed) }, \ - { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_state_quaternion_t, pitchspeed) }, \ - { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_hil_state_quaternion_t, yawspeed) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_hil_state_quaternion_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_hil_state_quaternion_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 44, offsetof(mavlink_hil_state_quaternion_t, alt) }, \ - { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_hil_state_quaternion_t, vx) }, \ - { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_hil_state_quaternion_t, vy) }, \ - { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_hil_state_quaternion_t, vz) }, \ - { "ind_airspeed", NULL, MAVLINK_TYPE_UINT16_T, 0, 54, offsetof(mavlink_hil_state_quaternion_t, ind_airspeed) }, \ - { "true_airspeed", NULL, MAVLINK_TYPE_UINT16_T, 0, 56, offsetof(mavlink_hil_state_quaternion_t, true_airspeed) }, \ - { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 58, offsetof(mavlink_hil_state_quaternion_t, xacc) }, \ - { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 60, offsetof(mavlink_hil_state_quaternion_t, yacc) }, \ - { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 62, offsetof(mavlink_hil_state_quaternion_t, zacc) }, \ - } \ -} - - -/** - * @brief Pack a hil_state_quaternion message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion - * @param rollspeed Body frame roll / phi angular speed (rad/s) - * @param pitchspeed Body frame pitch / theta angular speed (rad/s) - * @param yawspeed Body frame yaw / psi angular speed (rad/s) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) - * @param vx Ground X Speed (Latitude), expressed as m/s * 100 - * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param ind_airspeed Indicated airspeed, expressed as m/s * 100 - * @param true_airspeed True airspeed, expressed as m/s * 100 - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_state_quaternion_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 24, rollspeed); - _mav_put_float(buf, 28, pitchspeed); - _mav_put_float(buf, 32, yawspeed); - _mav_put_int32_t(buf, 36, lat); - _mav_put_int32_t(buf, 40, lon); - _mav_put_int32_t(buf, 44, alt); - _mav_put_int16_t(buf, 48, vx); - _mav_put_int16_t(buf, 50, vy); - _mav_put_int16_t(buf, 52, vz); - _mav_put_uint16_t(buf, 54, ind_airspeed); - _mav_put_uint16_t(buf, 56, true_airspeed); - _mav_put_int16_t(buf, 58, xacc); - _mav_put_int16_t(buf, 60, yacc); - _mav_put_int16_t(buf, 62, zacc); - _mav_put_float_array(buf, 8, attitude_quaternion, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); -#else - mavlink_hil_state_quaternion_t packet; - packet.time_usec = time_usec; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.ind_airspeed = ind_airspeed; - packet.true_airspeed = true_airspeed; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_STATE_QUATERNION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); -#endif -} - -/** - * @brief Pack a hil_state_quaternion message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion - * @param rollspeed Body frame roll / phi angular speed (rad/s) - * @param pitchspeed Body frame pitch / theta angular speed (rad/s) - * @param yawspeed Body frame yaw / psi angular speed (rad/s) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) - * @param vx Ground X Speed (Latitude), expressed as m/s * 100 - * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param ind_airspeed Indicated airspeed, expressed as m/s * 100 - * @param true_airspeed True airspeed, expressed as m/s * 100 - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_hil_state_quaternion_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,const float *attitude_quaternion,float rollspeed,float pitchspeed,float yawspeed,int32_t lat,int32_t lon,int32_t alt,int16_t vx,int16_t vy,int16_t vz,uint16_t ind_airspeed,uint16_t true_airspeed,int16_t xacc,int16_t yacc,int16_t zacc) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 24, rollspeed); - _mav_put_float(buf, 28, pitchspeed); - _mav_put_float(buf, 32, yawspeed); - _mav_put_int32_t(buf, 36, lat); - _mav_put_int32_t(buf, 40, lon); - _mav_put_int32_t(buf, 44, alt); - _mav_put_int16_t(buf, 48, vx); - _mav_put_int16_t(buf, 50, vy); - _mav_put_int16_t(buf, 52, vz); - _mav_put_uint16_t(buf, 54, ind_airspeed); - _mav_put_uint16_t(buf, 56, true_airspeed); - _mav_put_int16_t(buf, 58, xacc); - _mav_put_int16_t(buf, 60, yacc); - _mav_put_int16_t(buf, 62, zacc); - _mav_put_float_array(buf, 8, attitude_quaternion, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); -#else - mavlink_hil_state_quaternion_t packet; - packet.time_usec = time_usec; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.ind_airspeed = ind_airspeed; - packet.true_airspeed = true_airspeed; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_HIL_STATE_QUATERNION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); -#endif -} - -/** - * @brief Encode a hil_state_quaternion struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param hil_state_quaternion C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_state_quaternion_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_quaternion_t* hil_state_quaternion) -{ - return mavlink_msg_hil_state_quaternion_pack(system_id, component_id, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc); -} - -/** - * @brief Encode a hil_state_quaternion struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param hil_state_quaternion C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_state_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_quaternion_t* hil_state_quaternion) -{ - return mavlink_msg_hil_state_quaternion_pack_chan(system_id, component_id, chan, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc); -} - -/** - * @brief Send a hil_state_quaternion message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion - * @param rollspeed Body frame roll / phi angular speed (rad/s) - * @param pitchspeed Body frame pitch / theta angular speed (rad/s) - * @param yawspeed Body frame yaw / psi angular speed (rad/s) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) - * @param vx Ground X Speed (Latitude), expressed as m/s * 100 - * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param ind_airspeed Indicated airspeed, expressed as m/s * 100 - * @param true_airspeed True airspeed, expressed as m/s * 100 - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_hil_state_quaternion_send(mavlink_channel_t chan, uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 24, rollspeed); - _mav_put_float(buf, 28, pitchspeed); - _mav_put_float(buf, 32, yawspeed); - _mav_put_int32_t(buf, 36, lat); - _mav_put_int32_t(buf, 40, lon); - _mav_put_int32_t(buf, 44, alt); - _mav_put_int16_t(buf, 48, vx); - _mav_put_int16_t(buf, 50, vy); - _mav_put_int16_t(buf, 52, vz); - _mav_put_uint16_t(buf, 54, ind_airspeed); - _mav_put_uint16_t(buf, 56, true_airspeed); - _mav_put_int16_t(buf, 58, xacc); - _mav_put_int16_t(buf, 60, yacc); - _mav_put_int16_t(buf, 62, zacc); - _mav_put_float_array(buf, 8, attitude_quaternion, 4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); -#endif -#else - mavlink_hil_state_quaternion_t packet; - packet.time_usec = time_usec; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.ind_airspeed = ind_airspeed; - packet.true_airspeed = true_airspeed; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_hil_state_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 24, rollspeed); - _mav_put_float(buf, 28, pitchspeed); - _mav_put_float(buf, 32, yawspeed); - _mav_put_int32_t(buf, 36, lat); - _mav_put_int32_t(buf, 40, lon); - _mav_put_int32_t(buf, 44, alt); - _mav_put_int16_t(buf, 48, vx); - _mav_put_int16_t(buf, 50, vy); - _mav_put_int16_t(buf, 52, vz); - _mav_put_uint16_t(buf, 54, ind_airspeed); - _mav_put_uint16_t(buf, 56, true_airspeed); - _mav_put_int16_t(buf, 58, xacc); - _mav_put_int16_t(buf, 60, yacc); - _mav_put_int16_t(buf, 62, zacc); - _mav_put_float_array(buf, 8, attitude_quaternion, 4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); -#endif -#else - mavlink_hil_state_quaternion_t *packet = (mavlink_hil_state_quaternion_t *)msgbuf; - packet->time_usec = time_usec; - packet->rollspeed = rollspeed; - packet->pitchspeed = pitchspeed; - packet->yawspeed = yawspeed; - packet->lat = lat; - packet->lon = lon; - packet->alt = alt; - packet->vx = vx; - packet->vy = vy; - packet->vz = vz; - packet->ind_airspeed = ind_airspeed; - packet->true_airspeed = true_airspeed; - packet->xacc = xacc; - packet->yacc = yacc; - packet->zacc = zacc; - mav_array_memcpy(packet->attitude_quaternion, attitude_quaternion, sizeof(float)*4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE HIL_STATE_QUATERNION UNPACKING - - -/** - * @brief Get field time_usec from hil_state_quaternion message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_hil_state_quaternion_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field attitude_quaternion from hil_state_quaternion message - * - * @return Vehicle attitude expressed as normalized quaternion - */ -static inline uint16_t mavlink_msg_hil_state_quaternion_get_attitude_quaternion(const mavlink_message_t* msg, float *attitude_quaternion) -{ - return _MAV_RETURN_float_array(msg, attitude_quaternion, 4, 8); -} - -/** - * @brief Get field rollspeed from hil_state_quaternion message - * - * @return Body frame roll / phi angular speed (rad/s) - */ -static inline float mavlink_msg_hil_state_quaternion_get_rollspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field pitchspeed from hil_state_quaternion message - * - * @return Body frame pitch / theta angular speed (rad/s) - */ -static inline float mavlink_msg_hil_state_quaternion_get_pitchspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field yawspeed from hil_state_quaternion message - * - * @return Body frame yaw / psi angular speed (rad/s) - */ -static inline float mavlink_msg_hil_state_quaternion_get_yawspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field lat from hil_state_quaternion message - * - * @return Latitude, expressed as * 1E7 - */ -static inline int32_t mavlink_msg_hil_state_quaternion_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 36); -} - -/** - * @brief Get field lon from hil_state_quaternion message - * - * @return Longitude, expressed as * 1E7 - */ -static inline int32_t mavlink_msg_hil_state_quaternion_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 40); -} - -/** - * @brief Get field alt from hil_state_quaternion message - * - * @return Altitude in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_hil_state_quaternion_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 44); -} - -/** - * @brief Get field vx from hil_state_quaternion message - * - * @return Ground X Speed (Latitude), expressed as m/s * 100 - */ -static inline int16_t mavlink_msg_hil_state_quaternion_get_vx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 48); -} - -/** - * @brief Get field vy from hil_state_quaternion message - * - * @return Ground Y Speed (Longitude), expressed as m/s * 100 - */ -static inline int16_t mavlink_msg_hil_state_quaternion_get_vy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 50); -} - -/** - * @brief Get field vz from hil_state_quaternion message - * - * @return Ground Z Speed (Altitude), expressed as m/s * 100 - */ -static inline int16_t mavlink_msg_hil_state_quaternion_get_vz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 52); -} - -/** - * @brief Get field ind_airspeed from hil_state_quaternion message - * - * @return Indicated airspeed, expressed as m/s * 100 - */ -static inline uint16_t mavlink_msg_hil_state_quaternion_get_ind_airspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 54); -} - -/** - * @brief Get field true_airspeed from hil_state_quaternion message - * - * @return True airspeed, expressed as m/s * 100 - */ -static inline uint16_t mavlink_msg_hil_state_quaternion_get_true_airspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 56); -} - -/** - * @brief Get field xacc from hil_state_quaternion message - * - * @return X acceleration (mg) - */ -static inline int16_t mavlink_msg_hil_state_quaternion_get_xacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 58); -} - -/** - * @brief Get field yacc from hil_state_quaternion message - * - * @return Y acceleration (mg) - */ -static inline int16_t mavlink_msg_hil_state_quaternion_get_yacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 60); -} - -/** - * @brief Get field zacc from hil_state_quaternion message - * - * @return Z acceleration (mg) - */ -static inline int16_t mavlink_msg_hil_state_quaternion_get_zacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 62); -} - -/** - * @brief Decode a hil_state_quaternion message into a struct - * - * @param msg The message to decode - * @param hil_state_quaternion C-struct to decode the message contents into - */ -static inline void mavlink_msg_hil_state_quaternion_decode(const mavlink_message_t* msg, mavlink_hil_state_quaternion_t* hil_state_quaternion) -{ -#if MAVLINK_NEED_BYTE_SWAP - hil_state_quaternion->time_usec = mavlink_msg_hil_state_quaternion_get_time_usec(msg); - mavlink_msg_hil_state_quaternion_get_attitude_quaternion(msg, hil_state_quaternion->attitude_quaternion); - hil_state_quaternion->rollspeed = mavlink_msg_hil_state_quaternion_get_rollspeed(msg); - hil_state_quaternion->pitchspeed = mavlink_msg_hil_state_quaternion_get_pitchspeed(msg); - hil_state_quaternion->yawspeed = mavlink_msg_hil_state_quaternion_get_yawspeed(msg); - hil_state_quaternion->lat = mavlink_msg_hil_state_quaternion_get_lat(msg); - hil_state_quaternion->lon = mavlink_msg_hil_state_quaternion_get_lon(msg); - hil_state_quaternion->alt = mavlink_msg_hil_state_quaternion_get_alt(msg); - hil_state_quaternion->vx = mavlink_msg_hil_state_quaternion_get_vx(msg); - hil_state_quaternion->vy = mavlink_msg_hil_state_quaternion_get_vy(msg); - hil_state_quaternion->vz = mavlink_msg_hil_state_quaternion_get_vz(msg); - hil_state_quaternion->ind_airspeed = mavlink_msg_hil_state_quaternion_get_ind_airspeed(msg); - hil_state_quaternion->true_airspeed = mavlink_msg_hil_state_quaternion_get_true_airspeed(msg); - hil_state_quaternion->xacc = mavlink_msg_hil_state_quaternion_get_xacc(msg); - hil_state_quaternion->yacc = mavlink_msg_hil_state_quaternion_get_yacc(msg); - hil_state_quaternion->zacc = mavlink_msg_hil_state_quaternion_get_zacc(msg); -#else - memcpy(hil_state_quaternion, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h deleted file mode 100644 index e18a94869..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE LOCAL_POSITION_NED PACKING - -#define MAVLINK_MSG_ID_LOCAL_POSITION_NED 32 - -typedef struct __mavlink_local_position_ned_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - float x; ///< X Position - float y; ///< Y Position - float z; ///< Z Position - float vx; ///< X Speed - float vy; ///< Y Speed - float vz; ///< Z Speed -} mavlink_local_position_ned_t; - -#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN 28 -#define MAVLINK_MSG_ID_32_LEN 28 - -#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC 185 -#define MAVLINK_MSG_ID_32_CRC 185 - - - -#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED { \ - "LOCAL_POSITION_NED", \ - 7, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_local_position_ned_t, time_boot_ms) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_ned_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_ned_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_ned_t, z) }, \ - { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_ned_t, vx) }, \ - { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_ned_t, vy) }, \ - { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_ned_t, vz) }, \ - } \ -} - - -/** - * @brief Pack a local_position_ned message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param vx X Speed - * @param vy Y Speed - * @param vz Z Speed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_local_position_ned_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, x); - _mav_put_float(buf, 8, y); - _mav_put_float(buf, 12, z); - _mav_put_float(buf, 16, vx); - _mav_put_float(buf, 20, vy); - _mav_put_float(buf, 24, vz); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); -#else - mavlink_local_position_ned_t packet; - packet.time_boot_ms = time_boot_ms; - packet.x = x; - packet.y = y; - packet.z = z; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); -#endif -} - -/** - * @brief Pack a local_position_ned message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param vx X Speed - * @param vy Y Speed - * @param vz Z Speed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_local_position_ned_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,float x,float y,float z,float vx,float vy,float vz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, x); - _mav_put_float(buf, 8, y); - _mav_put_float(buf, 12, z); - _mav_put_float(buf, 16, vx); - _mav_put_float(buf, 20, vy); - _mav_put_float(buf, 24, vz); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); -#else - mavlink_local_position_ned_t packet; - packet.time_boot_ms = time_boot_ms; - packet.x = x; - packet.y = y; - packet.z = z; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); -#endif -} - -/** - * @brief Encode a local_position_ned struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param local_position_ned C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_local_position_ned_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_ned_t* local_position_ned) -{ - return mavlink_msg_local_position_ned_pack(system_id, component_id, msg, local_position_ned->time_boot_ms, local_position_ned->x, local_position_ned->y, local_position_ned->z, local_position_ned->vx, local_position_ned->vy, local_position_ned->vz); -} - -/** - * @brief Encode a local_position_ned struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param local_position_ned C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_local_position_ned_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_t* local_position_ned) -{ - return mavlink_msg_local_position_ned_pack_chan(system_id, component_id, chan, msg, local_position_ned->time_boot_ms, local_position_ned->x, local_position_ned->y, local_position_ned->z, local_position_ned->vx, local_position_ned->vy, local_position_ned->vz); -} - -/** - * @brief Send a local_position_ned message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param vx X Speed - * @param vy Y Speed - * @param vz Z Speed - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_local_position_ned_send(mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, x); - _mav_put_float(buf, 8, y); - _mav_put_float(buf, 12, z); - _mav_put_float(buf, 16, vx); - _mav_put_float(buf, 20, vy); - _mav_put_float(buf, 24, vz); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); -#endif -#else - mavlink_local_position_ned_t packet; - packet.time_boot_ms = time_boot_ms; - packet.x = x; - packet.y = y; - packet.z = z; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_local_position_ned_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, x); - _mav_put_float(buf, 8, y); - _mav_put_float(buf, 12, z); - _mav_put_float(buf, 16, vx); - _mav_put_float(buf, 20, vy); - _mav_put_float(buf, 24, vz); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); -#endif -#else - mavlink_local_position_ned_t *packet = (mavlink_local_position_ned_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->x = x; - packet->y = y; - packet->z = z; - packet->vx = vx; - packet->vy = vy; - packet->vz = vz; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE LOCAL_POSITION_NED UNPACKING - - -/** - * @brief Get field time_boot_ms from local_position_ned message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_local_position_ned_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field x from local_position_ned message - * - * @return X Position - */ -static inline float mavlink_msg_local_position_ned_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field y from local_position_ned message - * - * @return Y Position - */ -static inline float mavlink_msg_local_position_ned_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field z from local_position_ned message - * - * @return Z Position - */ -static inline float mavlink_msg_local_position_ned_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field vx from local_position_ned message - * - * @return X Speed - */ -static inline float mavlink_msg_local_position_ned_get_vx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field vy from local_position_ned message - * - * @return Y Speed - */ -static inline float mavlink_msg_local_position_ned_get_vy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field vz from local_position_ned message - * - * @return Z Speed - */ -static inline float mavlink_msg_local_position_ned_get_vz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Decode a local_position_ned message into a struct - * - * @param msg The message to decode - * @param local_position_ned C-struct to decode the message contents into - */ -static inline void mavlink_msg_local_position_ned_decode(const mavlink_message_t* msg, mavlink_local_position_ned_t* local_position_ned) -{ -#if MAVLINK_NEED_BYTE_SWAP - local_position_ned->time_boot_ms = mavlink_msg_local_position_ned_get_time_boot_ms(msg); - local_position_ned->x = mavlink_msg_local_position_ned_get_x(msg); - local_position_ned->y = mavlink_msg_local_position_ned_get_y(msg); - local_position_ned->z = mavlink_msg_local_position_ned_get_z(msg); - local_position_ned->vx = mavlink_msg_local_position_ned_get_vx(msg); - local_position_ned->vy = mavlink_msg_local_position_ned_get_vy(msg); - local_position_ned->vz = mavlink_msg_local_position_ned_get_vz(msg); -#else - memcpy(local_position_ned, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h deleted file mode 100644 index af7d195b0..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET PACKING - -#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET 89 - -typedef struct __mavlink_local_position_ned_system_global_offset_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - float x; ///< X Position - float y; ///< Y Position - float z; ///< Z Position - float roll; ///< Roll - float pitch; ///< Pitch - float yaw; ///< Yaw -} mavlink_local_position_ned_system_global_offset_t; - -#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN 28 -#define MAVLINK_MSG_ID_89_LEN 28 - -#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC 231 -#define MAVLINK_MSG_ID_89_CRC 231 - - - -#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET { \ - "LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET", \ - 7, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_local_position_ned_system_global_offset_t, time_boot_ms) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_ned_system_global_offset_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_ned_system_global_offset_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_ned_system_global_offset_t, z) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_ned_system_global_offset_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_ned_system_global_offset_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_ned_system_global_offset_t, yaw) }, \ - } \ -} - - -/** - * @brief Pack a local_position_ned_system_global_offset message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param roll Roll - * @param pitch Pitch - * @param yaw Yaw - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, x); - _mav_put_float(buf, 8, y); - _mav_put_float(buf, 12, z); - _mav_put_float(buf, 16, roll); - _mav_put_float(buf, 20, pitch); - _mav_put_float(buf, 24, yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); -#else - mavlink_local_position_ned_system_global_offset_t packet; - packet.time_boot_ms = time_boot_ms; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); -#endif -} - -/** - * @brief Pack a local_position_ned_system_global_offset message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param roll Roll - * @param pitch Pitch - * @param yaw Yaw - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,float x,float y,float z,float roll,float pitch,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, x); - _mav_put_float(buf, 8, y); - _mav_put_float(buf, 12, z); - _mav_put_float(buf, 16, roll); - _mav_put_float(buf, 20, pitch); - _mav_put_float(buf, 24, yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); -#else - mavlink_local_position_ned_system_global_offset_t packet; - packet.time_boot_ms = time_boot_ms; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); -#endif -} - -/** - * @brief Encode a local_position_ned_system_global_offset struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param local_position_ned_system_global_offset C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset) -{ - return mavlink_msg_local_position_ned_system_global_offset_pack(system_id, component_id, msg, local_position_ned_system_global_offset->time_boot_ms, local_position_ned_system_global_offset->x, local_position_ned_system_global_offset->y, local_position_ned_system_global_offset->z, local_position_ned_system_global_offset->roll, local_position_ned_system_global_offset->pitch, local_position_ned_system_global_offset->yaw); -} - -/** - * @brief Encode a local_position_ned_system_global_offset struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param local_position_ned_system_global_offset C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset) -{ - return mavlink_msg_local_position_ned_system_global_offset_pack_chan(system_id, component_id, chan, msg, local_position_ned_system_global_offset->time_boot_ms, local_position_ned_system_global_offset->x, local_position_ned_system_global_offset->y, local_position_ned_system_global_offset->z, local_position_ned_system_global_offset->roll, local_position_ned_system_global_offset->pitch, local_position_ned_system_global_offset->yaw); -} - -/** - * @brief Send a local_position_ned_system_global_offset message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param roll Roll - * @param pitch Pitch - * @param yaw Yaw - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_local_position_ned_system_global_offset_send(mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, x); - _mav_put_float(buf, 8, y); - _mav_put_float(buf, 12, z); - _mav_put_float(buf, 16, roll); - _mav_put_float(buf, 20, pitch); - _mav_put_float(buf, 24, yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); -#endif -#else - mavlink_local_position_ned_system_global_offset_t packet; - packet.time_boot_ms = time_boot_ms; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_local_position_ned_system_global_offset_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, x); - _mav_put_float(buf, 8, y); - _mav_put_float(buf, 12, z); - _mav_put_float(buf, 16, roll); - _mav_put_float(buf, 20, pitch); - _mav_put_float(buf, 24, yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); -#endif -#else - mavlink_local_position_ned_system_global_offset_t *packet = (mavlink_local_position_ned_system_global_offset_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->x = x; - packet->y = y; - packet->z = z; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET UNPACKING - - -/** - * @brief Get field time_boot_ms from local_position_ned_system_global_offset message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_local_position_ned_system_global_offset_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field x from local_position_ned_system_global_offset message - * - * @return X Position - */ -static inline float mavlink_msg_local_position_ned_system_global_offset_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field y from local_position_ned_system_global_offset message - * - * @return Y Position - */ -static inline float mavlink_msg_local_position_ned_system_global_offset_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field z from local_position_ned_system_global_offset message - * - * @return Z Position - */ -static inline float mavlink_msg_local_position_ned_system_global_offset_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field roll from local_position_ned_system_global_offset message - * - * @return Roll - */ -static inline float mavlink_msg_local_position_ned_system_global_offset_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field pitch from local_position_ned_system_global_offset message - * - * @return Pitch - */ -static inline float mavlink_msg_local_position_ned_system_global_offset_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field yaw from local_position_ned_system_global_offset message - * - * @return Yaw - */ -static inline float mavlink_msg_local_position_ned_system_global_offset_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Decode a local_position_ned_system_global_offset message into a struct - * - * @param msg The message to decode - * @param local_position_ned_system_global_offset C-struct to decode the message contents into - */ -static inline void mavlink_msg_local_position_ned_system_global_offset_decode(const mavlink_message_t* msg, mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset) -{ -#if MAVLINK_NEED_BYTE_SWAP - local_position_ned_system_global_offset->time_boot_ms = mavlink_msg_local_position_ned_system_global_offset_get_time_boot_ms(msg); - local_position_ned_system_global_offset->x = mavlink_msg_local_position_ned_system_global_offset_get_x(msg); - local_position_ned_system_global_offset->y = mavlink_msg_local_position_ned_system_global_offset_get_y(msg); - local_position_ned_system_global_offset->z = mavlink_msg_local_position_ned_system_global_offset_get_z(msg); - local_position_ned_system_global_offset->roll = mavlink_msg_local_position_ned_system_global_offset_get_roll(msg); - local_position_ned_system_global_offset->pitch = mavlink_msg_local_position_ned_system_global_offset_get_pitch(msg); - local_position_ned_system_global_offset->yaw = mavlink_msg_local_position_ned_system_global_offset_get_yaw(msg); -#else - memcpy(local_position_ned_system_global_offset, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h deleted file mode 100644 index 8dae721be..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE LOCAL_POSITION_SETPOINT PACKING - -#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT 51 - -typedef struct __mavlink_local_position_setpoint_t -{ - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< Desired yaw angle - uint8_t coordinate_frame; ///< Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU -} mavlink_local_position_setpoint_t; - -#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN 17 -#define MAVLINK_MSG_ID_51_LEN 17 - -#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC 223 -#define MAVLINK_MSG_ID_51_CRC 223 - - - -#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT { \ - "LOCAL_POSITION_SETPOINT", \ - 5, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_local_position_setpoint_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_setpoint_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_setpoint_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_setpoint_t, yaw) }, \ - { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_local_position_setpoint_t, coordinate_frame) }, \ - } \ -} - - -/** - * @brief Pack a local_position_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t coordinate_frame, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#else - mavlink_local_position_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a local_position_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t coordinate_frame,float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#else - mavlink_local_position_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a local_position_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param local_position_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_setpoint_t* local_position_setpoint) -{ - return mavlink_msg_local_position_setpoint_pack(system_id, component_id, msg, local_position_setpoint->coordinate_frame, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw); -} - -/** - * @brief Encode a local_position_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param local_position_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_local_position_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_setpoint_t* local_position_setpoint) -{ - return mavlink_msg_local_position_setpoint_pack_chan(system_id, component_id, chan, msg, local_position_setpoint->coordinate_frame, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw); -} - -/** - * @brief Send a local_position_setpoint message - * @param chan MAVLink channel to send the message - * - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, uint8_t coordinate_frame, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, coordinate_frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif -#else - mavlink_local_position_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_local_position_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t coordinate_frame, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, coordinate_frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif -#else - mavlink_local_position_setpoint_t *packet = (mavlink_local_position_setpoint_t *)msgbuf; - packet->x = x; - packet->y = y; - packet->z = z; - packet->yaw = yaw; - packet->coordinate_frame = coordinate_frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE LOCAL_POSITION_SETPOINT UNPACKING - - -/** - * @brief Get field coordinate_frame from local_position_setpoint message - * - * @return Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - */ -static inline uint8_t mavlink_msg_local_position_setpoint_get_coordinate_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field x from local_position_setpoint message - * - * @return x position - */ -static inline float mavlink_msg_local_position_setpoint_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from local_position_setpoint message - * - * @return y position - */ -static inline float mavlink_msg_local_position_setpoint_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from local_position_setpoint message - * - * @return z position - */ -static inline float mavlink_msg_local_position_setpoint_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from local_position_setpoint message - * - * @return Desired yaw angle - */ -static inline float mavlink_msg_local_position_setpoint_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a local_position_setpoint message into a struct - * - * @param msg The message to decode - * @param local_position_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_local_position_setpoint_decode(const mavlink_message_t* msg, mavlink_local_position_setpoint_t* local_position_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - local_position_setpoint->x = mavlink_msg_local_position_setpoint_get_x(msg); - local_position_setpoint->y = mavlink_msg_local_position_setpoint_get_y(msg); - local_position_setpoint->z = mavlink_msg_local_position_setpoint_get_z(msg); - local_position_setpoint->yaw = mavlink_msg_local_position_setpoint_get_yaw(msg); - local_position_setpoint->coordinate_frame = mavlink_msg_local_position_setpoint_get_coordinate_frame(msg); -#else - memcpy(local_position_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h deleted file mode 100644 index 48641f3eb..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h +++ /dev/null @@ -1,273 +0,0 @@ -// MESSAGE LOG_DATA PACKING - -#define MAVLINK_MSG_ID_LOG_DATA 120 - -typedef struct __mavlink_log_data_t -{ - uint32_t ofs; ///< Offset into the log - uint16_t id; ///< Log id (from LOG_ENTRY reply) - uint8_t count; ///< Number of bytes (zero for end of log) - uint8_t data[90]; ///< log data -} mavlink_log_data_t; - -#define MAVLINK_MSG_ID_LOG_DATA_LEN 97 -#define MAVLINK_MSG_ID_120_LEN 97 - -#define MAVLINK_MSG_ID_LOG_DATA_CRC 134 -#define MAVLINK_MSG_ID_120_CRC 134 - -#define MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN 90 - -#define MAVLINK_MESSAGE_INFO_LOG_DATA { \ - "LOG_DATA", \ - 4, \ - { { "ofs", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_data_t, ofs) }, \ - { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_log_data_t, id) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_log_data_t, count) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 90, 7, offsetof(mavlink_log_data_t, data) }, \ - } \ -} - - -/** - * @brief Pack a log_data message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param id Log id (from LOG_ENTRY reply) - * @param ofs Offset into the log - * @param count Number of bytes (zero for end of log) - * @param data log data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_log_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_DATA_LEN]; - _mav_put_uint32_t(buf, 0, ofs); - _mav_put_uint16_t(buf, 4, id); - _mav_put_uint8_t(buf, 6, count); - _mav_put_uint8_t_array(buf, 7, data, 90); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_DATA_LEN); -#else - mavlink_log_data_t packet; - packet.ofs = ofs; - packet.id = id; - packet.count = count; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOG_DATA; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_DATA_LEN); -#endif -} - -/** - * @brief Pack a log_data message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param id Log id (from LOG_ENTRY reply) - * @param ofs Offset into the log - * @param count Number of bytes (zero for end of log) - * @param data log data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_log_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t id,uint32_t ofs,uint8_t count,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_DATA_LEN]; - _mav_put_uint32_t(buf, 0, ofs); - _mav_put_uint16_t(buf, 4, id); - _mav_put_uint8_t(buf, 6, count); - _mav_put_uint8_t_array(buf, 7, data, 90); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_DATA_LEN); -#else - mavlink_log_data_t packet; - packet.ofs = ofs; - packet.id = id; - packet.count = count; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOG_DATA; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_DATA_LEN); -#endif -} - -/** - * @brief Encode a log_data struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param log_data C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_log_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_data_t* log_data) -{ - return mavlink_msg_log_data_pack(system_id, component_id, msg, log_data->id, log_data->ofs, log_data->count, log_data->data); -} - -/** - * @brief Encode a log_data struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param log_data C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_log_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_data_t* log_data) -{ - return mavlink_msg_log_data_pack_chan(system_id, component_id, chan, msg, log_data->id, log_data->ofs, log_data->count, log_data->data); -} - -/** - * @brief Send a log_data message - * @param chan MAVLink channel to send the message - * - * @param id Log id (from LOG_ENTRY reply) - * @param ofs Offset into the log - * @param count Number of bytes (zero for end of log) - * @param data log data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_log_data_send(mavlink_channel_t chan, uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_DATA_LEN]; - _mav_put_uint32_t(buf, 0, ofs); - _mav_put_uint16_t(buf, 4, id); - _mav_put_uint8_t(buf, 6, count); - _mav_put_uint8_t_array(buf, 7, data, 90); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN); -#endif -#else - mavlink_log_data_t packet; - packet.ofs = ofs; - packet.id = id; - packet.count = count; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_DATA_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_LOG_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_log_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, ofs); - _mav_put_uint16_t(buf, 4, id); - _mav_put_uint8_t(buf, 6, count); - _mav_put_uint8_t_array(buf, 7, data, 90); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN); -#endif -#else - mavlink_log_data_t *packet = (mavlink_log_data_t *)msgbuf; - packet->ofs = ofs; - packet->id = id; - packet->count = count; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*90); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE LOG_DATA UNPACKING - - -/** - * @brief Get field id from log_data message - * - * @return Log id (from LOG_ENTRY reply) - */ -static inline uint16_t mavlink_msg_log_data_get_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field ofs from log_data message - * - * @return Offset into the log - */ -static inline uint32_t mavlink_msg_log_data_get_ofs(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field count from log_data message - * - * @return Number of bytes (zero for end of log) - */ -static inline uint8_t mavlink_msg_log_data_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field data from log_data message - * - * @return log data - */ -static inline uint16_t mavlink_msg_log_data_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 90, 7); -} - -/** - * @brief Decode a log_data message into a struct - * - * @param msg The message to decode - * @param log_data C-struct to decode the message contents into - */ -static inline void mavlink_msg_log_data_decode(const mavlink_message_t* msg, mavlink_log_data_t* log_data) -{ -#if MAVLINK_NEED_BYTE_SWAP - log_data->ofs = mavlink_msg_log_data_get_ofs(msg); - log_data->id = mavlink_msg_log_data_get_id(msg); - log_data->count = mavlink_msg_log_data_get_count(msg); - mavlink_msg_log_data_get_data(msg, log_data->data); -#else - memcpy(log_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_DATA_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h deleted file mode 100644 index 8ecaec3ae..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE LOG_ENTRY PACKING - -#define MAVLINK_MSG_ID_LOG_ENTRY 118 - -typedef struct __mavlink_log_entry_t -{ - uint32_t time_utc; ///< UTC timestamp of log in seconds since 1970, or 0 if not available - uint32_t size; ///< Size of the log (may be approximate) in bytes - uint16_t id; ///< Log id - uint16_t num_logs; ///< Total number of logs - uint16_t last_log_num; ///< High log number -} mavlink_log_entry_t; - -#define MAVLINK_MSG_ID_LOG_ENTRY_LEN 14 -#define MAVLINK_MSG_ID_118_LEN 14 - -#define MAVLINK_MSG_ID_LOG_ENTRY_CRC 56 -#define MAVLINK_MSG_ID_118_CRC 56 - - - -#define MAVLINK_MESSAGE_INFO_LOG_ENTRY { \ - "LOG_ENTRY", \ - 5, \ - { { "time_utc", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_entry_t, time_utc) }, \ - { "size", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_log_entry_t, size) }, \ - { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_log_entry_t, id) }, \ - { "num_logs", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_log_entry_t, num_logs) }, \ - { "last_log_num", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_log_entry_t, last_log_num) }, \ - } \ -} - - -/** - * @brief Pack a log_entry message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param id Log id - * @param num_logs Total number of logs - * @param last_log_num High log number - * @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available - * @param size Size of the log (may be approximate) in bytes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_log_entry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN]; - _mav_put_uint32_t(buf, 0, time_utc); - _mav_put_uint32_t(buf, 4, size); - _mav_put_uint16_t(buf, 8, id); - _mav_put_uint16_t(buf, 10, num_logs); - _mav_put_uint16_t(buf, 12, last_log_num); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); -#else - mavlink_log_entry_t packet; - packet.time_utc = time_utc; - packet.size = size; - packet.id = id; - packet.num_logs = num_logs; - packet.last_log_num = last_log_num; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOG_ENTRY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ENTRY_LEN); -#endif -} - -/** - * @brief Pack a log_entry message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param id Log id - * @param num_logs Total number of logs - * @param last_log_num High log number - * @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available - * @param size Size of the log (may be approximate) in bytes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_log_entry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t id,uint16_t num_logs,uint16_t last_log_num,uint32_t time_utc,uint32_t size) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN]; - _mav_put_uint32_t(buf, 0, time_utc); - _mav_put_uint32_t(buf, 4, size); - _mav_put_uint16_t(buf, 8, id); - _mav_put_uint16_t(buf, 10, num_logs); - _mav_put_uint16_t(buf, 12, last_log_num); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); -#else - mavlink_log_entry_t packet; - packet.time_utc = time_utc; - packet.size = size; - packet.id = id; - packet.num_logs = num_logs; - packet.last_log_num = last_log_num; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOG_ENTRY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ENTRY_LEN); -#endif -} - -/** - * @brief Encode a log_entry struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param log_entry C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_log_entry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_entry_t* log_entry) -{ - return mavlink_msg_log_entry_pack(system_id, component_id, msg, log_entry->id, log_entry->num_logs, log_entry->last_log_num, log_entry->time_utc, log_entry->size); -} - -/** - * @brief Encode a log_entry struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param log_entry C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_log_entry_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_entry_t* log_entry) -{ - return mavlink_msg_log_entry_pack_chan(system_id, component_id, chan, msg, log_entry->id, log_entry->num_logs, log_entry->last_log_num, log_entry->time_utc, log_entry->size); -} - -/** - * @brief Send a log_entry message - * @param chan MAVLink channel to send the message - * - * @param id Log id - * @param num_logs Total number of logs - * @param last_log_num High log number - * @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available - * @param size Size of the log (may be approximate) in bytes - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_log_entry_send(mavlink_channel_t chan, uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN]; - _mav_put_uint32_t(buf, 0, time_utc); - _mav_put_uint32_t(buf, 4, size); - _mav_put_uint16_t(buf, 8, id); - _mav_put_uint16_t(buf, 10, num_logs); - _mav_put_uint16_t(buf, 12, last_log_num); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); -#endif -#else - mavlink_log_entry_t packet; - packet.time_utc = time_utc; - packet.size = size; - packet.id = id; - packet.num_logs = num_logs; - packet.last_log_num = last_log_num; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)&packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)&packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_LOG_ENTRY_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_log_entry_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_utc); - _mav_put_uint32_t(buf, 4, size); - _mav_put_uint16_t(buf, 8, id); - _mav_put_uint16_t(buf, 10, num_logs); - _mav_put_uint16_t(buf, 12, last_log_num); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); -#endif -#else - mavlink_log_entry_t *packet = (mavlink_log_entry_t *)msgbuf; - packet->time_utc = time_utc; - packet->size = size; - packet->id = id; - packet->num_logs = num_logs; - packet->last_log_num = last_log_num; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE LOG_ENTRY UNPACKING - - -/** - * @brief Get field id from log_entry message - * - * @return Log id - */ -static inline uint16_t mavlink_msg_log_entry_get_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field num_logs from log_entry message - * - * @return Total number of logs - */ -static inline uint16_t mavlink_msg_log_entry_get_num_logs(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Get field last_log_num from log_entry message - * - * @return High log number - */ -static inline uint16_t mavlink_msg_log_entry_get_last_log_num(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field time_utc from log_entry message - * - * @return UTC timestamp of log in seconds since 1970, or 0 if not available - */ -static inline uint32_t mavlink_msg_log_entry_get_time_utc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field size from log_entry message - * - * @return Size of the log (may be approximate) in bytes - */ -static inline uint32_t mavlink_msg_log_entry_get_size(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 4); -} - -/** - * @brief Decode a log_entry message into a struct - * - * @param msg The message to decode - * @param log_entry C-struct to decode the message contents into - */ -static inline void mavlink_msg_log_entry_decode(const mavlink_message_t* msg, mavlink_log_entry_t* log_entry) -{ -#if MAVLINK_NEED_BYTE_SWAP - log_entry->time_utc = mavlink_msg_log_entry_get_time_utc(msg); - log_entry->size = mavlink_msg_log_entry_get_size(msg); - log_entry->id = mavlink_msg_log_entry_get_id(msg); - log_entry->num_logs = mavlink_msg_log_entry_get_num_logs(msg); - log_entry->last_log_num = mavlink_msg_log_entry_get_last_log_num(msg); -#else - memcpy(log_entry, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_ENTRY_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h deleted file mode 100644 index 957c4d4cc..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE LOG_ERASE PACKING - -#define MAVLINK_MSG_ID_LOG_ERASE 121 - -typedef struct __mavlink_log_erase_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_log_erase_t; - -#define MAVLINK_MSG_ID_LOG_ERASE_LEN 2 -#define MAVLINK_MSG_ID_121_LEN 2 - -#define MAVLINK_MSG_ID_LOG_ERASE_CRC 237 -#define MAVLINK_MSG_ID_121_CRC 237 - - - -#define MAVLINK_MESSAGE_INFO_LOG_ERASE { \ - "LOG_ERASE", \ - 2, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_log_erase_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_log_erase_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a log_erase message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_log_erase_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); -#else - mavlink_log_erase_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOG_ERASE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ERASE_LEN); -#endif -} - -/** - * @brief Pack a log_erase message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_log_erase_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); -#else - mavlink_log_erase_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOG_ERASE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ERASE_LEN); -#endif -} - -/** - * @brief Encode a log_erase struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param log_erase C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_log_erase_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase) -{ - return mavlink_msg_log_erase_pack(system_id, component_id, msg, log_erase->target_system, log_erase->target_component); -} - -/** - * @brief Encode a log_erase struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param log_erase C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_log_erase_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase) -{ - return mavlink_msg_log_erase_pack_chan(system_id, component_id, chan, msg, log_erase->target_system, log_erase->target_component); -} - -/** - * @brief Send a log_erase message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_log_erase_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); -#endif -#else - mavlink_log_erase_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)&packet, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)&packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_LOG_ERASE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_log_erase_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); -#endif -#else - mavlink_log_erase_t *packet = (mavlink_log_erase_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE LOG_ERASE UNPACKING - - -/** - * @brief Get field target_system from log_erase message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_log_erase_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from log_erase message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_log_erase_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a log_erase message into a struct - * - * @param msg The message to decode - * @param log_erase C-struct to decode the message contents into - */ -static inline void mavlink_msg_log_erase_decode(const mavlink_message_t* msg, mavlink_log_erase_t* log_erase) -{ -#if MAVLINK_NEED_BYTE_SWAP - log_erase->target_system = mavlink_msg_log_erase_get_target_system(msg); - log_erase->target_component = mavlink_msg_log_erase_get_target_component(msg); -#else - memcpy(log_erase, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_ERASE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h deleted file mode 100644 index ef5cbb67c..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE LOG_REQUEST_DATA PACKING - -#define MAVLINK_MSG_ID_LOG_REQUEST_DATA 119 - -typedef struct __mavlink_log_request_data_t -{ - uint32_t ofs; ///< Offset into the log - uint32_t count; ///< Number of bytes - uint16_t id; ///< Log id (from LOG_ENTRY reply) - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_log_request_data_t; - -#define MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN 12 -#define MAVLINK_MSG_ID_119_LEN 12 - -#define MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC 116 -#define MAVLINK_MSG_ID_119_CRC 116 - - - -#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA { \ - "LOG_REQUEST_DATA", \ - 5, \ - { { "ofs", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_request_data_t, ofs) }, \ - { "count", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_log_request_data_t, count) }, \ - { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_log_request_data_t, id) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_log_request_data_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_log_request_data_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a log_request_data message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param id Log id (from LOG_ENTRY reply) - * @param ofs Offset into the log - * @param count Number of bytes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_log_request_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN]; - _mav_put_uint32_t(buf, 0, ofs); - _mav_put_uint32_t(buf, 4, count); - _mav_put_uint16_t(buf, 8, id); - _mav_put_uint8_t(buf, 10, target_system); - _mav_put_uint8_t(buf, 11, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); -#else - mavlink_log_request_data_t packet; - packet.ofs = ofs; - packet.count = count; - packet.id = id; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_DATA; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); -#endif -} - -/** - * @brief Pack a log_request_data message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param id Log id (from LOG_ENTRY reply) - * @param ofs Offset into the log - * @param count Number of bytes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_log_request_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t id,uint32_t ofs,uint32_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN]; - _mav_put_uint32_t(buf, 0, ofs); - _mav_put_uint32_t(buf, 4, count); - _mav_put_uint16_t(buf, 8, id); - _mav_put_uint8_t(buf, 10, target_system); - _mav_put_uint8_t(buf, 11, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); -#else - mavlink_log_request_data_t packet; - packet.ofs = ofs; - packet.count = count; - packet.id = id; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_DATA; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); -#endif -} - -/** - * @brief Encode a log_request_data struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param log_request_data C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_log_request_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_data_t* log_request_data) -{ - return mavlink_msg_log_request_data_pack(system_id, component_id, msg, log_request_data->target_system, log_request_data->target_component, log_request_data->id, log_request_data->ofs, log_request_data->count); -} - -/** - * @brief Encode a log_request_data struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param log_request_data C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_log_request_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_data_t* log_request_data) -{ - return mavlink_msg_log_request_data_pack_chan(system_id, component_id, chan, msg, log_request_data->target_system, log_request_data->target_component, log_request_data->id, log_request_data->ofs, log_request_data->count); -} - -/** - * @brief Send a log_request_data message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param id Log id (from LOG_ENTRY reply) - * @param ofs Offset into the log - * @param count Number of bytes - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_log_request_data_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN]; - _mav_put_uint32_t(buf, 0, ofs); - _mav_put_uint32_t(buf, 4, count); - _mav_put_uint16_t(buf, 8, id); - _mav_put_uint8_t(buf, 10, target_system); - _mav_put_uint8_t(buf, 11, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); -#endif -#else - mavlink_log_request_data_t packet; - packet.ofs = ofs; - packet.count = count; - packet.id = id; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_log_request_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, ofs); - _mav_put_uint32_t(buf, 4, count); - _mav_put_uint16_t(buf, 8, id); - _mav_put_uint8_t(buf, 10, target_system); - _mav_put_uint8_t(buf, 11, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); -#endif -#else - mavlink_log_request_data_t *packet = (mavlink_log_request_data_t *)msgbuf; - packet->ofs = ofs; - packet->count = count; - packet->id = id; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE LOG_REQUEST_DATA UNPACKING - - -/** - * @brief Get field target_system from log_request_data message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_log_request_data_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field target_component from log_request_data message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_log_request_data_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field id from log_request_data message - * - * @return Log id (from LOG_ENTRY reply) - */ -static inline uint16_t mavlink_msg_log_request_data_get_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field ofs from log_request_data message - * - * @return Offset into the log - */ -static inline uint32_t mavlink_msg_log_request_data_get_ofs(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field count from log_request_data message - * - * @return Number of bytes - */ -static inline uint32_t mavlink_msg_log_request_data_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 4); -} - -/** - * @brief Decode a log_request_data message into a struct - * - * @param msg The message to decode - * @param log_request_data C-struct to decode the message contents into - */ -static inline void mavlink_msg_log_request_data_decode(const mavlink_message_t* msg, mavlink_log_request_data_t* log_request_data) -{ -#if MAVLINK_NEED_BYTE_SWAP - log_request_data->ofs = mavlink_msg_log_request_data_get_ofs(msg); - log_request_data->count = mavlink_msg_log_request_data_get_count(msg); - log_request_data->id = mavlink_msg_log_request_data_get_id(msg); - log_request_data->target_system = mavlink_msg_log_request_data_get_target_system(msg); - log_request_data->target_component = mavlink_msg_log_request_data_get_target_component(msg); -#else - memcpy(log_request_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h deleted file mode 100644 index 23fcca29f..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE LOG_REQUEST_END PACKING - -#define MAVLINK_MSG_ID_LOG_REQUEST_END 122 - -typedef struct __mavlink_log_request_end_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_log_request_end_t; - -#define MAVLINK_MSG_ID_LOG_REQUEST_END_LEN 2 -#define MAVLINK_MSG_ID_122_LEN 2 - -#define MAVLINK_MSG_ID_LOG_REQUEST_END_CRC 203 -#define MAVLINK_MSG_ID_122_CRC 203 - - - -#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_END { \ - "LOG_REQUEST_END", \ - 2, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_log_request_end_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_log_request_end_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a log_request_end message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_log_request_end_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); -#else - mavlink_log_request_end_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_END; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); -#endif -} - -/** - * @brief Pack a log_request_end message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_log_request_end_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); -#else - mavlink_log_request_end_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_END; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); -#endif -} - -/** - * @brief Encode a log_request_end struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param log_request_end C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_log_request_end_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_end_t* log_request_end) -{ - return mavlink_msg_log_request_end_pack(system_id, component_id, msg, log_request_end->target_system, log_request_end->target_component); -} - -/** - * @brief Encode a log_request_end struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param log_request_end C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_log_request_end_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_end_t* log_request_end) -{ - return mavlink_msg_log_request_end_pack_chan(system_id, component_id, chan, msg, log_request_end->target_system, log_request_end->target_component); -} - -/** - * @brief Send a log_request_end message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_log_request_end_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); -#endif -#else - mavlink_log_request_end_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_LOG_REQUEST_END_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_log_request_end_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); -#endif -#else - mavlink_log_request_end_t *packet = (mavlink_log_request_end_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE LOG_REQUEST_END UNPACKING - - -/** - * @brief Get field target_system from log_request_end message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_log_request_end_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from log_request_end message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_log_request_end_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a log_request_end message into a struct - * - * @param msg The message to decode - * @param log_request_end C-struct to decode the message contents into - */ -static inline void mavlink_msg_log_request_end_decode(const mavlink_message_t* msg, mavlink_log_request_end_t* log_request_end) -{ -#if MAVLINK_NEED_BYTE_SWAP - log_request_end->target_system = mavlink_msg_log_request_end_get_target_system(msg); - log_request_end->target_component = mavlink_msg_log_request_end_get_target_component(msg); -#else - memcpy(log_request_end, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h deleted file mode 100644 index e511b5312..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE LOG_REQUEST_LIST PACKING - -#define MAVLINK_MSG_ID_LOG_REQUEST_LIST 117 - -typedef struct __mavlink_log_request_list_t -{ - uint16_t start; ///< First log id (0 for first available) - uint16_t end; ///< Last log id (0xffff for last available) - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_log_request_list_t; - -#define MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN 6 -#define MAVLINK_MSG_ID_117_LEN 6 - -#define MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC 128 -#define MAVLINK_MSG_ID_117_CRC 128 - - - -#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST { \ - "LOG_REQUEST_LIST", \ - 4, \ - { { "start", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_log_request_list_t, start) }, \ - { "end", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_log_request_list_t, end) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_log_request_list_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_log_request_list_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a log_request_list message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param start First log id (0 for first available) - * @param end Last log id (0xffff for last available) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_log_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN]; - _mav_put_uint16_t(buf, 0, start); - _mav_put_uint16_t(buf, 2, end); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); -#else - mavlink_log_request_list_t packet; - packet.start = start; - packet.end = end; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); -#endif -} - -/** - * @brief Pack a log_request_list message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param start First log id (0 for first available) - * @param end Last log id (0xffff for last available) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_log_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t start,uint16_t end) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN]; - _mav_put_uint16_t(buf, 0, start); - _mav_put_uint16_t(buf, 2, end); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); -#else - mavlink_log_request_list_t packet; - packet.start = start; - packet.end = end; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); -#endif -} - -/** - * @brief Encode a log_request_list struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param log_request_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_log_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_list_t* log_request_list) -{ - return mavlink_msg_log_request_list_pack(system_id, component_id, msg, log_request_list->target_system, log_request_list->target_component, log_request_list->start, log_request_list->end); -} - -/** - * @brief Encode a log_request_list struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param log_request_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_log_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_list_t* log_request_list) -{ - return mavlink_msg_log_request_list_pack_chan(system_id, component_id, chan, msg, log_request_list->target_system, log_request_list->target_component, log_request_list->start, log_request_list->end); -} - -/** - * @brief Send a log_request_list message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param start First log id (0 for first available) - * @param end Last log id (0xffff for last available) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_log_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN]; - _mav_put_uint16_t(buf, 0, start); - _mav_put_uint16_t(buf, 2, end); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); -#endif -#else - mavlink_log_request_list_t packet; - packet.start = start; - packet.end = end; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_log_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, start); - _mav_put_uint16_t(buf, 2, end); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); -#endif -#else - mavlink_log_request_list_t *packet = (mavlink_log_request_list_t *)msgbuf; - packet->start = start; - packet->end = end; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE LOG_REQUEST_LIST UNPACKING - - -/** - * @brief Get field target_system from log_request_list message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_log_request_list_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field target_component from log_request_list message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_log_request_list_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field start from log_request_list message - * - * @return First log id (0 for first available) - */ -static inline uint16_t mavlink_msg_log_request_list_get_start(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field end from log_request_list message - * - * @return Last log id (0xffff for last available) - */ -static inline uint16_t mavlink_msg_log_request_list_get_end(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Decode a log_request_list message into a struct - * - * @param msg The message to decode - * @param log_request_list C-struct to decode the message contents into - */ -static inline void mavlink_msg_log_request_list_decode(const mavlink_message_t* msg, mavlink_log_request_list_t* log_request_list) -{ -#if MAVLINK_NEED_BYTE_SWAP - log_request_list->start = mavlink_msg_log_request_list_get_start(msg); - log_request_list->end = mavlink_msg_log_request_list_get_end(msg); - log_request_list->target_system = mavlink_msg_log_request_list_get_target_system(msg); - log_request_list->target_component = mavlink_msg_log_request_list_get_target_component(msg); -#else - memcpy(log_request_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h deleted file mode 100644 index e93b759d0..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE MANUAL_CONTROL PACKING - -#define MAVLINK_MSG_ID_MANUAL_CONTROL 69 - -typedef struct __mavlink_manual_control_t -{ - int16_t x; ///< X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. - int16_t y; ///< Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. - int16_t z; ///< Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. - int16_t r; ///< R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. - uint16_t buttons; ///< A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. - uint8_t target; ///< The system to be controlled. -} mavlink_manual_control_t; - -#define MAVLINK_MSG_ID_MANUAL_CONTROL_LEN 11 -#define MAVLINK_MSG_ID_69_LEN 11 - -#define MAVLINK_MSG_ID_MANUAL_CONTROL_CRC 243 -#define MAVLINK_MSG_ID_69_CRC 243 - - - -#define MAVLINK_MESSAGE_INFO_MANUAL_CONTROL { \ - "MANUAL_CONTROL", \ - 6, \ - { { "x", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_manual_control_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_manual_control_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_manual_control_t, z) }, \ - { "r", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_manual_control_t, r) }, \ - { "buttons", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_manual_control_t, buttons) }, \ - { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_manual_control_t, target) }, \ - } \ -} - - -/** - * @brief Pack a manual_control message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target The system to be controlled. - * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. - * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. - * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. - * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. - * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN]; - _mav_put_int16_t(buf, 0, x); - _mav_put_int16_t(buf, 2, y); - _mav_put_int16_t(buf, 4, z); - _mav_put_int16_t(buf, 6, r); - _mav_put_uint16_t(buf, 8, buttons); - _mav_put_uint8_t(buf, 10, target); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); -#else - mavlink_manual_control_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.r = r; - packet.buttons = buttons; - packet.target = target; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); -#endif -} - -/** - * @brief Pack a manual_control message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target The system to be controlled. - * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. - * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. - * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. - * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. - * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,int16_t x,int16_t y,int16_t z,int16_t r,uint16_t buttons) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN]; - _mav_put_int16_t(buf, 0, x); - _mav_put_int16_t(buf, 2, y); - _mav_put_int16_t(buf, 4, z); - _mav_put_int16_t(buf, 6, r); - _mav_put_uint16_t(buf, 8, buttons); - _mav_put_uint8_t(buf, 10, target); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); -#else - mavlink_manual_control_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.r = r; - packet.buttons = buttons; - packet.target = target; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); -#endif -} - -/** - * @brief Encode a manual_control struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param manual_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control) -{ - return mavlink_msg_manual_control_pack(system_id, component_id, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons); -} - -/** - * @brief Encode a manual_control struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param manual_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_manual_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control) -{ - return mavlink_msg_manual_control_pack_chan(system_id, component_id, chan, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons); -} - -/** - * @brief Send a manual_control message - * @param chan MAVLink channel to send the message - * - * @param target The system to be controlled. - * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. - * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. - * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. - * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. - * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN]; - _mav_put_int16_t(buf, 0, x); - _mav_put_int16_t(buf, 2, y); - _mav_put_int16_t(buf, 4, z); - _mav_put_int16_t(buf, 6, r); - _mav_put_uint16_t(buf, 8, buttons); - _mav_put_uint8_t(buf, 10, target); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); -#endif -#else - mavlink_manual_control_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.r = r; - packet.buttons = buttons; - packet.target = target; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MANUAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_manual_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int16_t(buf, 0, x); - _mav_put_int16_t(buf, 2, y); - _mav_put_int16_t(buf, 4, z); - _mav_put_int16_t(buf, 6, r); - _mav_put_uint16_t(buf, 8, buttons); - _mav_put_uint8_t(buf, 10, target); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); -#endif -#else - mavlink_manual_control_t *packet = (mavlink_manual_control_t *)msgbuf; - packet->x = x; - packet->y = y; - packet->z = z; - packet->r = r; - packet->buttons = buttons; - packet->target = target; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MANUAL_CONTROL UNPACKING - - -/** - * @brief Get field target from manual_control message - * - * @return The system to be controlled. - */ -static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field x from manual_control message - * - * @return X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. - */ -static inline int16_t mavlink_msg_manual_control_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 0); -} - -/** - * @brief Get field y from manual_control message - * - * @return Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. - */ -static inline int16_t mavlink_msg_manual_control_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 2); -} - -/** - * @brief Get field z from manual_control message - * - * @return Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. - */ -static inline int16_t mavlink_msg_manual_control_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 4); -} - -/** - * @brief Get field r from manual_control message - * - * @return R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. - */ -static inline int16_t mavlink_msg_manual_control_get_r(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 6); -} - -/** - * @brief Get field buttons from manual_control message - * - * @return A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. - */ -static inline uint16_t mavlink_msg_manual_control_get_buttons(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Decode a manual_control message into a struct - * - * @param msg The message to decode - * @param manual_control C-struct to decode the message contents into - */ -static inline void mavlink_msg_manual_control_decode(const mavlink_message_t* msg, mavlink_manual_control_t* manual_control) -{ -#if MAVLINK_NEED_BYTE_SWAP - manual_control->x = mavlink_msg_manual_control_get_x(msg); - manual_control->y = mavlink_msg_manual_control_get_y(msg); - manual_control->z = mavlink_msg_manual_control_get_z(msg); - manual_control->r = mavlink_msg_manual_control_get_r(msg); - manual_control->buttons = mavlink_msg_manual_control_get_buttons(msg); - manual_control->target = mavlink_msg_manual_control_get_target(msg); -#else - memcpy(manual_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h deleted file mode 100644 index b27626726..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE MANUAL_SETPOINT PACKING - -#define MAVLINK_MSG_ID_MANUAL_SETPOINT 81 - -typedef struct __mavlink_manual_setpoint_t -{ - uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot - float roll; ///< Desired roll rate in radians per second - float pitch; ///< Desired pitch rate in radians per second - float yaw; ///< Desired yaw rate in radians per second - float thrust; ///< Collective thrust, normalized to 0 .. 1 - uint8_t mode_switch; ///< Flight mode switch position, 0.. 255 - uint8_t manual_override_switch; ///< Override mode switch position, 0.. 255 -} mavlink_manual_setpoint_t; - -#define MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN 22 -#define MAVLINK_MSG_ID_81_LEN 22 - -#define MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC 106 -#define MAVLINK_MSG_ID_81_CRC 106 - - - -#define MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT { \ - "MANUAL_SETPOINT", \ - 7, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_manual_setpoint_t, time_boot_ms) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_manual_setpoint_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_manual_setpoint_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_manual_setpoint_t, yaw) }, \ - { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_manual_setpoint_t, thrust) }, \ - { "mode_switch", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_manual_setpoint_t, mode_switch) }, \ - { "manual_override_switch", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_manual_setpoint_t, manual_override_switch) }, \ - } \ -} - - -/** - * @brief Pack a manual_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll Desired roll rate in radians per second - * @param pitch Desired pitch rate in radians per second - * @param yaw Desired yaw rate in radians per second - * @param thrust Collective thrust, normalized to 0 .. 1 - * @param mode_switch Flight mode switch position, 0.. 255 - * @param manual_override_switch Override mode switch position, 0.. 255 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_manual_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust, uint8_t mode_switch, uint8_t manual_override_switch) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, thrust); - _mav_put_uint8_t(buf, 20, mode_switch); - _mav_put_uint8_t(buf, 21, manual_override_switch); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); -#else - mavlink_manual_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.mode_switch = mode_switch; - packet.manual_override_switch = manual_override_switch; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MANUAL_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a manual_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll Desired roll rate in radians per second - * @param pitch Desired pitch rate in radians per second - * @param yaw Desired yaw rate in radians per second - * @param thrust Collective thrust, normalized to 0 .. 1 - * @param mode_switch Flight mode switch position, 0.. 255 - * @param manual_override_switch Override mode switch position, 0.. 255 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_manual_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,float roll,float pitch,float yaw,float thrust,uint8_t mode_switch,uint8_t manual_override_switch) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, thrust); - _mav_put_uint8_t(buf, 20, mode_switch); - _mav_put_uint8_t(buf, 21, manual_override_switch); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); -#else - mavlink_manual_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.mode_switch = mode_switch; - packet.manual_override_switch = manual_override_switch; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MANUAL_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a manual_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param manual_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_manual_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_manual_setpoint_t* manual_setpoint) -{ - return mavlink_msg_manual_setpoint_pack(system_id, component_id, msg, manual_setpoint->time_boot_ms, manual_setpoint->roll, manual_setpoint->pitch, manual_setpoint->yaw, manual_setpoint->thrust, manual_setpoint->mode_switch, manual_setpoint->manual_override_switch); -} - -/** - * @brief Encode a manual_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param manual_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_manual_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_setpoint_t* manual_setpoint) -{ - return mavlink_msg_manual_setpoint_pack_chan(system_id, component_id, chan, msg, manual_setpoint->time_boot_ms, manual_setpoint->roll, manual_setpoint->pitch, manual_setpoint->yaw, manual_setpoint->thrust, manual_setpoint->mode_switch, manual_setpoint->manual_override_switch); -} - -/** - * @brief Send a manual_setpoint message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll Desired roll rate in radians per second - * @param pitch Desired pitch rate in radians per second - * @param yaw Desired yaw rate in radians per second - * @param thrust Collective thrust, normalized to 0 .. 1 - * @param mode_switch Flight mode switch position, 0.. 255 - * @param manual_override_switch Override mode switch position, 0.. 255 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_manual_setpoint_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust, uint8_t mode_switch, uint8_t manual_override_switch) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, thrust); - _mav_put_uint8_t(buf, 20, mode_switch); - _mav_put_uint8_t(buf, 21, manual_override_switch); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); -#endif -#else - mavlink_manual_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.mode_switch = mode_switch; - packet.manual_override_switch = manual_override_switch; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_manual_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust, uint8_t mode_switch, uint8_t manual_override_switch) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, thrust); - _mav_put_uint8_t(buf, 20, mode_switch); - _mav_put_uint8_t(buf, 21, manual_override_switch); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); -#endif -#else - mavlink_manual_setpoint_t *packet = (mavlink_manual_setpoint_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - packet->thrust = thrust; - packet->mode_switch = mode_switch; - packet->manual_override_switch = manual_override_switch; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MANUAL_SETPOINT UNPACKING - - -/** - * @brief Get field time_boot_ms from manual_setpoint message - * - * @return Timestamp in milliseconds since system boot - */ -static inline uint32_t mavlink_msg_manual_setpoint_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field roll from manual_setpoint message - * - * @return Desired roll rate in radians per second - */ -static inline float mavlink_msg_manual_setpoint_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field pitch from manual_setpoint message - * - * @return Desired pitch rate in radians per second - */ -static inline float mavlink_msg_manual_setpoint_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from manual_setpoint message - * - * @return Desired yaw rate in radians per second - */ -static inline float mavlink_msg_manual_setpoint_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field thrust from manual_setpoint message - * - * @return Collective thrust, normalized to 0 .. 1 - */ -static inline float mavlink_msg_manual_setpoint_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field mode_switch from manual_setpoint message - * - * @return Flight mode switch position, 0.. 255 - */ -static inline uint8_t mavlink_msg_manual_setpoint_get_mode_switch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 20); -} - -/** - * @brief Get field manual_override_switch from manual_setpoint message - * - * @return Override mode switch position, 0.. 255 - */ -static inline uint8_t mavlink_msg_manual_setpoint_get_manual_override_switch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 21); -} - -/** - * @brief Decode a manual_setpoint message into a struct - * - * @param msg The message to decode - * @param manual_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_manual_setpoint_decode(const mavlink_message_t* msg, mavlink_manual_setpoint_t* manual_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - manual_setpoint->time_boot_ms = mavlink_msg_manual_setpoint_get_time_boot_ms(msg); - manual_setpoint->roll = mavlink_msg_manual_setpoint_get_roll(msg); - manual_setpoint->pitch = mavlink_msg_manual_setpoint_get_pitch(msg); - manual_setpoint->yaw = mavlink_msg_manual_setpoint_get_yaw(msg); - manual_setpoint->thrust = mavlink_msg_manual_setpoint_get_thrust(msg); - manual_setpoint->mode_switch = mavlink_msg_manual_setpoint_get_mode_switch(msg); - manual_setpoint->manual_override_switch = mavlink_msg_manual_setpoint_get_manual_override_switch(msg); -#else - memcpy(manual_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h deleted file mode 100644 index 2eb60cc0e..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h +++ /dev/null @@ -1,273 +0,0 @@ -// MESSAGE MEMORY_VECT PACKING - -#define MAVLINK_MSG_ID_MEMORY_VECT 249 - -typedef struct __mavlink_memory_vect_t -{ - uint16_t address; ///< Starting address of the debug variables - uint8_t ver; ///< Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - uint8_t type; ///< Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 - int8_t value[32]; ///< Memory contents at specified address -} mavlink_memory_vect_t; - -#define MAVLINK_MSG_ID_MEMORY_VECT_LEN 36 -#define MAVLINK_MSG_ID_249_LEN 36 - -#define MAVLINK_MSG_ID_MEMORY_VECT_CRC 204 -#define MAVLINK_MSG_ID_249_CRC 204 - -#define MAVLINK_MSG_MEMORY_VECT_FIELD_VALUE_LEN 32 - -#define MAVLINK_MESSAGE_INFO_MEMORY_VECT { \ - "MEMORY_VECT", \ - 4, \ - { { "address", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_memory_vect_t, address) }, \ - { "ver", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_memory_vect_t, ver) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_memory_vect_t, type) }, \ - { "value", NULL, MAVLINK_TYPE_INT8_T, 32, 4, offsetof(mavlink_memory_vect_t, value) }, \ - } \ -} - - -/** - * @brief Pack a memory_vect message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param address Starting address of the debug variables - * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 - * @param value Memory contents at specified address - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t address, uint8_t ver, uint8_t type, const int8_t *value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MEMORY_VECT_LEN]; - _mav_put_uint16_t(buf, 0, address); - _mav_put_uint8_t(buf, 2, ver); - _mav_put_uint8_t(buf, 3, type); - _mav_put_int8_t_array(buf, 4, value, 32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN); -#else - mavlink_memory_vect_t packet; - packet.address = address; - packet.ver = ver; - packet.type = type; - mav_array_memcpy(packet.value, value, sizeof(int8_t)*32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMORY_VECT_LEN); -#endif -} - -/** - * @brief Pack a memory_vect message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param address Starting address of the debug variables - * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 - * @param value Memory contents at specified address - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t address,uint8_t ver,uint8_t type,const int8_t *value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MEMORY_VECT_LEN]; - _mav_put_uint16_t(buf, 0, address); - _mav_put_uint8_t(buf, 2, ver); - _mav_put_uint8_t(buf, 3, type); - _mav_put_int8_t_array(buf, 4, value, 32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN); -#else - mavlink_memory_vect_t packet; - packet.address = address; - packet.ver = ver; - packet.type = type; - mav_array_memcpy(packet.value, value, sizeof(int8_t)*32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN); -#endif -} - -/** - * @brief Encode a memory_vect struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param memory_vect C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_memory_vect_t* memory_vect) -{ - return mavlink_msg_memory_vect_pack(system_id, component_id, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); -} - -/** - * @brief Encode a memory_vect struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param memory_vect C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_memory_vect_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_memory_vect_t* memory_vect) -{ - return mavlink_msg_memory_vect_pack_chan(system_id, component_id, chan, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); -} - -/** - * @brief Send a memory_vect message - * @param chan MAVLink channel to send the message - * - * @param address Starting address of the debug variables - * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 - * @param value Memory contents at specified address - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t address, uint8_t ver, uint8_t type, const int8_t *value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MEMORY_VECT_LEN]; - _mav_put_uint16_t(buf, 0, address); - _mav_put_uint8_t(buf, 2, ver); - _mav_put_uint8_t(buf, 3, type); - _mav_put_int8_t_array(buf, 4, value, 32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN); -#endif -#else - mavlink_memory_vect_t packet; - packet.address = address; - packet.ver = ver; - packet.type = type; - mav_array_memcpy(packet.value, value, sizeof(int8_t)*32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)&packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)&packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MEMORY_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_memory_vect_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t address, uint8_t ver, uint8_t type, const int8_t *value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, address); - _mav_put_uint8_t(buf, 2, ver); - _mav_put_uint8_t(buf, 3, type); - _mav_put_int8_t_array(buf, 4, value, 32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN); -#endif -#else - mavlink_memory_vect_t *packet = (mavlink_memory_vect_t *)msgbuf; - packet->address = address; - packet->ver = ver; - packet->type = type; - mav_array_memcpy(packet->value, value, sizeof(int8_t)*32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MEMORY_VECT UNPACKING - - -/** - * @brief Get field address from memory_vect message - * - * @return Starting address of the debug variables - */ -static inline uint16_t mavlink_msg_memory_vect_get_address(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field ver from memory_vect message - * - * @return Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - */ -static inline uint8_t mavlink_msg_memory_vect_get_ver(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field type from memory_vect message - * - * @return Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 - */ -static inline uint8_t mavlink_msg_memory_vect_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field value from memory_vect message - * - * @return Memory contents at specified address - */ -static inline uint16_t mavlink_msg_memory_vect_get_value(const mavlink_message_t* msg, int8_t *value) -{ - return _MAV_RETURN_int8_t_array(msg, value, 32, 4); -} - -/** - * @brief Decode a memory_vect message into a struct - * - * @param msg The message to decode - * @param memory_vect C-struct to decode the message contents into - */ -static inline void mavlink_msg_memory_vect_decode(const mavlink_message_t* msg, mavlink_memory_vect_t* memory_vect) -{ -#if MAVLINK_NEED_BYTE_SWAP - memory_vect->address = mavlink_msg_memory_vect_get_address(msg); - memory_vect->ver = mavlink_msg_memory_vect_get_ver(msg); - memory_vect->type = mavlink_msg_memory_vect_get_type(msg); - mavlink_msg_memory_vect_get_value(msg, memory_vect->value); -#else - memcpy(memory_vect, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MEMORY_VECT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h deleted file mode 100644 index 43afd00f7..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE MISSION_ACK PACKING - -#define MAVLINK_MSG_ID_MISSION_ACK 47 - -typedef struct __mavlink_mission_ack_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t type; ///< See MAV_MISSION_RESULT enum -} mavlink_mission_ack_t; - -#define MAVLINK_MSG_ID_MISSION_ACK_LEN 3 -#define MAVLINK_MSG_ID_47_LEN 3 - -#define MAVLINK_MSG_ID_MISSION_ACK_CRC 153 -#define MAVLINK_MSG_ID_47_CRC 153 - - - -#define MAVLINK_MESSAGE_INFO_MISSION_ACK { \ - "MISSION_ACK", \ - 3, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_ack_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_ack_t, target_component) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_ack_t, type) }, \ - } \ -} - - -/** - * @brief Pack a mission_ack message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param type See MAV_MISSION_RESULT enum - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, type); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); -#else - mavlink_mission_ack_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.type = type; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ACK_LEN); -#endif -} - -/** - * @brief Pack a mission_ack message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param type See MAV_MISSION_RESULT enum - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, type); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); -#else - mavlink_mission_ack_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.type = type; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ACK_LEN); -#endif -} - -/** - * @brief Encode a mission_ack struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mission_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) -{ - return mavlink_msg_mission_ack_pack(system_id, component_id, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type); -} - -/** - * @brief Encode a mission_ack struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mission_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) -{ - return mavlink_msg_mission_ack_pack_chan(system_id, component_id, chan, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type); -} - -/** - * @brief Send a mission_ack message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param type See MAV_MISSION_RESULT enum - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, type); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); -#endif -#else - mavlink_mission_ack_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.type = type; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MISSION_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, type); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); -#endif -#else - mavlink_mission_ack_t *packet = (mavlink_mission_ack_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - packet->type = type; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MISSION_ACK UNPACKING - - -/** - * @brief Get field target_system from mission_ack message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mission_ack_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from mission_ack message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mission_ack_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field type from mission_ack message - * - * @return See MAV_MISSION_RESULT enum - */ -static inline uint8_t mavlink_msg_mission_ack_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a mission_ack message into a struct - * - * @param msg The message to decode - * @param mission_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_mission_ack_decode(const mavlink_message_t* msg, mavlink_mission_ack_t* mission_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - mission_ack->target_system = mavlink_msg_mission_ack_get_target_system(msg); - mission_ack->target_component = mavlink_msg_mission_ack_get_target_component(msg); - mission_ack->type = mavlink_msg_mission_ack_get_type(msg); -#else - memcpy(mission_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_ACK_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h deleted file mode 100644 index 120986873..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE MISSION_CLEAR_ALL PACKING - -#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL 45 - -typedef struct __mavlink_mission_clear_all_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_mission_clear_all_t; - -#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN 2 -#define MAVLINK_MSG_ID_45_LEN 2 - -#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC 232 -#define MAVLINK_MSG_ID_45_CRC 232 - - - -#define MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL { \ - "MISSION_CLEAR_ALL", \ - 2, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_clear_all_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_clear_all_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a mission_clear_all message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); -#else - mavlink_mission_clear_all_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_CLEAR_ALL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); -#endif -} - -/** - * @brief Pack a mission_clear_all message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_clear_all_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); -#else - mavlink_mission_clear_all_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_CLEAR_ALL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); -#endif -} - -/** - * @brief Encode a mission_clear_all struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mission_clear_all C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) -{ - return mavlink_msg_mission_clear_all_pack(system_id, component_id, msg, mission_clear_all->target_system, mission_clear_all->target_component); -} - -/** - * @brief Encode a mission_clear_all struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mission_clear_all C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_clear_all_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) -{ - return mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, chan, msg, mission_clear_all->target_system, mission_clear_all->target_component); -} - -/** - * @brief Send a mission_clear_all message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); -#endif -#else - mavlink_mission_clear_all_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mission_clear_all_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); -#endif -#else - mavlink_mission_clear_all_t *packet = (mavlink_mission_clear_all_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MISSION_CLEAR_ALL UNPACKING - - -/** - * @brief Get field target_system from mission_clear_all message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mission_clear_all_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from mission_clear_all message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mission_clear_all_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a mission_clear_all message into a struct - * - * @param msg The message to decode - * @param mission_clear_all C-struct to decode the message contents into - */ -static inline void mavlink_msg_mission_clear_all_decode(const mavlink_message_t* msg, mavlink_mission_clear_all_t* mission_clear_all) -{ -#if MAVLINK_NEED_BYTE_SWAP - mission_clear_all->target_system = mavlink_msg_mission_clear_all_get_target_system(msg); - mission_clear_all->target_component = mavlink_msg_mission_clear_all_get_target_component(msg); -#else - memcpy(mission_clear_all, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h deleted file mode 100644 index 7e4748eae..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE MISSION_COUNT PACKING - -#define MAVLINK_MSG_ID_MISSION_COUNT 44 - -typedef struct __mavlink_mission_count_t -{ - uint16_t count; ///< Number of mission items in the sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_mission_count_t; - -#define MAVLINK_MSG_ID_MISSION_COUNT_LEN 4 -#define MAVLINK_MSG_ID_44_LEN 4 - -#define MAVLINK_MSG_ID_MISSION_COUNT_CRC 221 -#define MAVLINK_MSG_ID_44_CRC 221 - - - -#define MAVLINK_MESSAGE_INFO_MISSION_COUNT { \ - "MISSION_COUNT", \ - 3, \ - { { "count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_count_t, count) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_count_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_count_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a mission_count message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param count Number of mission items in the sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; - _mav_put_uint16_t(buf, 0, count); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); -#else - mavlink_mission_count_t packet; - packet.count = count; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_COUNT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_COUNT_LEN); -#endif -} - -/** - * @brief Pack a mission_count message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param count Number of mission items in the sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; - _mav_put_uint16_t(buf, 0, count); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); -#else - mavlink_mission_count_t packet; - packet.count = count; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_COUNT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_COUNT_LEN); -#endif -} - -/** - * @brief Encode a mission_count struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mission_count C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) -{ - return mavlink_msg_mission_count_pack(system_id, component_id, msg, mission_count->target_system, mission_count->target_component, mission_count->count); -} - -/** - * @brief Encode a mission_count struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mission_count C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_count_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) -{ - return mavlink_msg_mission_count_pack_chan(system_id, component_id, chan, msg, mission_count->target_system, mission_count->target_component, mission_count->count); -} - -/** - * @brief Send a mission_count message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param count Number of mission items in the sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; - _mav_put_uint16_t(buf, 0, count); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); -#endif -#else - mavlink_mission_count_t packet; - packet.count = count; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MISSION_COUNT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, count); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); -#endif -#else - mavlink_mission_count_t *packet = (mavlink_mission_count_t *)msgbuf; - packet->count = count; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MISSION_COUNT UNPACKING - - -/** - * @brief Get field target_system from mission_count message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mission_count_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from mission_count message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mission_count_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field count from mission_count message - * - * @return Number of mission items in the sequence - */ -static inline uint16_t mavlink_msg_mission_count_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a mission_count message into a struct - * - * @param msg The message to decode - * @param mission_count C-struct to decode the message contents into - */ -static inline void mavlink_msg_mission_count_decode(const mavlink_message_t* msg, mavlink_mission_count_t* mission_count) -{ -#if MAVLINK_NEED_BYTE_SWAP - mission_count->count = mavlink_msg_mission_count_get_count(msg); - mission_count->target_system = mavlink_msg_mission_count_get_target_system(msg); - mission_count->target_component = mavlink_msg_mission_count_get_target_component(msg); -#else - memcpy(mission_count, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_COUNT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h deleted file mode 100644 index 201b7a6fb..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h +++ /dev/null @@ -1,209 +0,0 @@ -// MESSAGE MISSION_CURRENT PACKING - -#define MAVLINK_MSG_ID_MISSION_CURRENT 42 - -typedef struct __mavlink_mission_current_t -{ - uint16_t seq; ///< Sequence -} mavlink_mission_current_t; - -#define MAVLINK_MSG_ID_MISSION_CURRENT_LEN 2 -#define MAVLINK_MSG_ID_42_LEN 2 - -#define MAVLINK_MSG_ID_MISSION_CURRENT_CRC 28 -#define MAVLINK_MSG_ID_42_CRC 28 - - - -#define MAVLINK_MESSAGE_INFO_MISSION_CURRENT { \ - "MISSION_CURRENT", \ - 1, \ - { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_current_t, seq) }, \ - } \ -} - - -/** - * @brief Pack a mission_current message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; - _mav_put_uint16_t(buf, 0, seq); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); -#else - mavlink_mission_current_t packet; - packet.seq = seq; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); -#endif -} - -/** - * @brief Pack a mission_current message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; - _mav_put_uint16_t(buf, 0, seq); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); -#else - mavlink_mission_current_t packet; - packet.seq = seq; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); -#endif -} - -/** - * @brief Encode a mission_current struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mission_current C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_current_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current) -{ - return mavlink_msg_mission_current_pack(system_id, component_id, msg, mission_current->seq); -} - -/** - * @brief Encode a mission_current struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mission_current C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current) -{ - return mavlink_msg_mission_current_pack_chan(system_id, component_id, chan, msg, mission_current->seq); -} - -/** - * @brief Send a mission_current message - * @param chan MAVLink channel to send the message - * - * @param seq Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; - _mav_put_uint16_t(buf, 0, seq); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); -#endif -#else - mavlink_mission_current_t packet; - packet.seq = seq; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MISSION_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mission_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, seq); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); -#endif -#else - mavlink_mission_current_t *packet = (mavlink_mission_current_t *)msgbuf; - packet->seq = seq; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MISSION_CURRENT UNPACKING - - -/** - * @brief Get field seq from mission_current message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_mission_current_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a mission_current message into a struct - * - * @param msg The message to decode - * @param mission_current C-struct to decode the message contents into - */ -static inline void mavlink_msg_mission_current_decode(const mavlink_message_t* msg, mavlink_mission_current_t* mission_current) -{ -#if MAVLINK_NEED_BYTE_SWAP - mission_current->seq = mavlink_msg_mission_current_get_seq(msg); -#else - memcpy(mission_current, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_CURRENT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h deleted file mode 100644 index ef9394f00..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h +++ /dev/null @@ -1,521 +0,0 @@ -// MESSAGE MISSION_ITEM PACKING - -#define MAVLINK_MSG_ID_MISSION_ITEM 39 - -typedef struct __mavlink_mission_item_t -{ - float param1; ///< PARAM1, see MAV_CMD enum - float param2; ///< PARAM2, see MAV_CMD enum - float param3; ///< PARAM3, see MAV_CMD enum - float param4; ///< PARAM4, see MAV_CMD enum - float x; ///< PARAM5 / local: x position, global: latitude - float y; ///< PARAM6 / y position: global: longitude - float z; ///< PARAM7 / z position: global: altitude (relative or absolute, depending on frame. - uint16_t seq; ///< Sequence - uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h - uint8_t current; ///< false:0, true:1 - uint8_t autocontinue; ///< autocontinue to next wp -} mavlink_mission_item_t; - -#define MAVLINK_MSG_ID_MISSION_ITEM_LEN 37 -#define MAVLINK_MSG_ID_39_LEN 37 - -#define MAVLINK_MSG_ID_MISSION_ITEM_CRC 254 -#define MAVLINK_MSG_ID_39_CRC 254 - - - -#define MAVLINK_MESSAGE_INFO_MISSION_ITEM { \ - "MISSION_ITEM", \ - 14, \ - { { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mission_item_t, param1) }, \ - { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mission_item_t, param2) }, \ - { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mission_item_t, param3) }, \ - { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mission_item_t, param4) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mission_item_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mission_item_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_t, z) }, \ - { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_t, seq) }, \ - { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_mission_item_t, command) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_t, target_component) }, \ - { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_mission_item_t, frame) }, \ - { "current", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_mission_item_t, current) }, \ - { "autocontinue", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_mission_item_t, autocontinue) }, \ - } \ -} - - -/** - * @brief Pack a mission_item message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h - * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp - * @param param1 PARAM1, see MAV_CMD enum - * @param param2 PARAM2, see MAV_CMD enum - * @param param3 PARAM3, see MAV_CMD enum - * @param param4 PARAM4, see MAV_CMD enum - * @param x PARAM5 / local: x position, global: latitude - * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_float(buf, 16, x); - _mav_put_float(buf, 20, y); - _mav_put_float(buf, 24, z); - _mav_put_uint16_t(buf, 28, seq); - _mav_put_uint16_t(buf, 30, command); - _mav_put_uint8_t(buf, 32, target_system); - _mav_put_uint8_t(buf, 33, target_component); - _mav_put_uint8_t(buf, 34, frame); - _mav_put_uint8_t(buf, 35, current); - _mav_put_uint8_t(buf, 36, autocontinue); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); -#else - mavlink_mission_item_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.x = x; - packet.y = y; - packet.z = z; - packet.seq = seq; - packet.command = command; - packet.target_system = target_system; - packet.target_component = target_component; - packet.frame = frame; - packet.current = current; - packet.autocontinue = autocontinue; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_LEN); -#endif -} - -/** - * @brief Pack a mission_item message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h - * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp - * @param param1 PARAM1, see MAV_CMD enum - * @param param2 PARAM2, see MAV_CMD enum - * @param param3 PARAM3, see MAV_CMD enum - * @param param4 PARAM4, see MAV_CMD enum - * @param x PARAM5 / local: x position, global: latitude - * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,float x,float y,float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_float(buf, 16, x); - _mav_put_float(buf, 20, y); - _mav_put_float(buf, 24, z); - _mav_put_uint16_t(buf, 28, seq); - _mav_put_uint16_t(buf, 30, command); - _mav_put_uint8_t(buf, 32, target_system); - _mav_put_uint8_t(buf, 33, target_component); - _mav_put_uint8_t(buf, 34, frame); - _mav_put_uint8_t(buf, 35, current); - _mav_put_uint8_t(buf, 36, autocontinue); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); -#else - mavlink_mission_item_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.x = x; - packet.y = y; - packet.z = z; - packet.seq = seq; - packet.command = command; - packet.target_system = target_system; - packet.target_component = target_component; - packet.frame = frame; - packet.current = current; - packet.autocontinue = autocontinue; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_LEN); -#endif -} - -/** - * @brief Encode a mission_item struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mission_item C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item) -{ - return mavlink_msg_mission_item_pack(system_id, component_id, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); -} - -/** - * @brief Encode a mission_item struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mission_item C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item) -{ - return mavlink_msg_mission_item_pack_chan(system_id, component_id, chan, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); -} - -/** - * @brief Send a mission_item message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h - * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp - * @param param1 PARAM1, see MAV_CMD enum - * @param param2 PARAM2, see MAV_CMD enum - * @param param3 PARAM3, see MAV_CMD enum - * @param param4 PARAM4, see MAV_CMD enum - * @param x PARAM5 / local: x position, global: latitude - * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_float(buf, 16, x); - _mav_put_float(buf, 20, y); - _mav_put_float(buf, 24, z); - _mav_put_uint16_t(buf, 28, seq); - _mav_put_uint16_t(buf, 30, command); - _mav_put_uint8_t(buf, 32, target_system); - _mav_put_uint8_t(buf, 33, target_component); - _mav_put_uint8_t(buf, 34, frame); - _mav_put_uint8_t(buf, 35, current); - _mav_put_uint8_t(buf, 36, autocontinue); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); -#endif -#else - mavlink_mission_item_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.x = x; - packet.y = y; - packet.z = z; - packet.seq = seq; - packet.command = command; - packet.target_system = target_system; - packet.target_component = target_component; - packet.frame = frame; - packet.current = current; - packet.autocontinue = autocontinue; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MISSION_ITEM_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_float(buf, 16, x); - _mav_put_float(buf, 20, y); - _mav_put_float(buf, 24, z); - _mav_put_uint16_t(buf, 28, seq); - _mav_put_uint16_t(buf, 30, command); - _mav_put_uint8_t(buf, 32, target_system); - _mav_put_uint8_t(buf, 33, target_component); - _mav_put_uint8_t(buf, 34, frame); - _mav_put_uint8_t(buf, 35, current); - _mav_put_uint8_t(buf, 36, autocontinue); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); -#endif -#else - mavlink_mission_item_t *packet = (mavlink_mission_item_t *)msgbuf; - packet->param1 = param1; - packet->param2 = param2; - packet->param3 = param3; - packet->param4 = param4; - packet->x = x; - packet->y = y; - packet->z = z; - packet->seq = seq; - packet->command = command; - packet->target_system = target_system; - packet->target_component = target_component; - packet->frame = frame; - packet->current = current; - packet->autocontinue = autocontinue; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MISSION_ITEM UNPACKING - - -/** - * @brief Get field target_system from mission_item message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mission_item_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field target_component from mission_item message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mission_item_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 33); -} - -/** - * @brief Get field seq from mission_item message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_mission_item_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 28); -} - -/** - * @brief Get field frame from mission_item message - * - * @return The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h - */ -static inline uint8_t mavlink_msg_mission_item_get_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 34); -} - -/** - * @brief Get field command from mission_item message - * - * @return The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs - */ -static inline uint16_t mavlink_msg_mission_item_get_command(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 30); -} - -/** - * @brief Get field current from mission_item message - * - * @return false:0, true:1 - */ -static inline uint8_t mavlink_msg_mission_item_get_current(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 35); -} - -/** - * @brief Get field autocontinue from mission_item message - * - * @return autocontinue to next wp - */ -static inline uint8_t mavlink_msg_mission_item_get_autocontinue(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 36); -} - -/** - * @brief Get field param1 from mission_item message - * - * @return PARAM1, see MAV_CMD enum - */ -static inline float mavlink_msg_mission_item_get_param1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field param2 from mission_item message - * - * @return PARAM2, see MAV_CMD enum - */ -static inline float mavlink_msg_mission_item_get_param2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field param3 from mission_item message - * - * @return PARAM3, see MAV_CMD enum - */ -static inline float mavlink_msg_mission_item_get_param3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field param4 from mission_item message - * - * @return PARAM4, see MAV_CMD enum - */ -static inline float mavlink_msg_mission_item_get_param4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field x from mission_item message - * - * @return PARAM5 / local: x position, global: latitude - */ -static inline float mavlink_msg_mission_item_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field y from mission_item message - * - * @return PARAM6 / y position: global: longitude - */ -static inline float mavlink_msg_mission_item_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field z from mission_item message - * - * @return PARAM7 / z position: global: altitude (relative or absolute, depending on frame. - */ -static inline float mavlink_msg_mission_item_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Decode a mission_item message into a struct - * - * @param msg The message to decode - * @param mission_item C-struct to decode the message contents into - */ -static inline void mavlink_msg_mission_item_decode(const mavlink_message_t* msg, mavlink_mission_item_t* mission_item) -{ -#if MAVLINK_NEED_BYTE_SWAP - mission_item->param1 = mavlink_msg_mission_item_get_param1(msg); - mission_item->param2 = mavlink_msg_mission_item_get_param2(msg); - mission_item->param3 = mavlink_msg_mission_item_get_param3(msg); - mission_item->param4 = mavlink_msg_mission_item_get_param4(msg); - mission_item->x = mavlink_msg_mission_item_get_x(msg); - mission_item->y = mavlink_msg_mission_item_get_y(msg); - mission_item->z = mavlink_msg_mission_item_get_z(msg); - mission_item->seq = mavlink_msg_mission_item_get_seq(msg); - mission_item->command = mavlink_msg_mission_item_get_command(msg); - mission_item->target_system = mavlink_msg_mission_item_get_target_system(msg); - mission_item->target_component = mavlink_msg_mission_item_get_target_component(msg); - mission_item->frame = mavlink_msg_mission_item_get_frame(msg); - mission_item->current = mavlink_msg_mission_item_get_current(msg); - mission_item->autocontinue = mavlink_msg_mission_item_get_autocontinue(msg); -#else - memcpy(mission_item, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_ITEM_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h deleted file mode 100644 index 9dfa28043..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h +++ /dev/null @@ -1,209 +0,0 @@ -// MESSAGE MISSION_ITEM_REACHED PACKING - -#define MAVLINK_MSG_ID_MISSION_ITEM_REACHED 46 - -typedef struct __mavlink_mission_item_reached_t -{ - uint16_t seq; ///< Sequence -} mavlink_mission_item_reached_t; - -#define MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN 2 -#define MAVLINK_MSG_ID_46_LEN 2 - -#define MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC 11 -#define MAVLINK_MSG_ID_46_CRC 11 - - - -#define MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED { \ - "MISSION_ITEM_REACHED", \ - 1, \ - { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_item_reached_t, seq) }, \ - } \ -} - - -/** - * @brief Pack a mission_item_reached message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_item_reached_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN]; - _mav_put_uint16_t(buf, 0, seq); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); -#else - mavlink_mission_item_reached_t packet; - packet.seq = seq; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); -#endif -} - -/** - * @brief Pack a mission_item_reached message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_item_reached_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN]; - _mav_put_uint16_t(buf, 0, seq); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); -#else - mavlink_mission_item_reached_t packet; - packet.seq = seq; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); -#endif -} - -/** - * @brief Encode a mission_item_reached struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mission_item_reached C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_item_reached_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_reached_t* mission_item_reached) -{ - return mavlink_msg_mission_item_reached_pack(system_id, component_id, msg, mission_item_reached->seq); -} - -/** - * @brief Encode a mission_item_reached struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mission_item_reached C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_item_reached_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_reached_t* mission_item_reached) -{ - return mavlink_msg_mission_item_reached_pack_chan(system_id, component_id, chan, msg, mission_item_reached->seq); -} - -/** - * @brief Send a mission_item_reached message - * @param chan MAVLink channel to send the message - * - * @param seq Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mission_item_reached_send(mavlink_channel_t chan, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN]; - _mav_put_uint16_t(buf, 0, seq); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); -#endif -#else - mavlink_mission_item_reached_t packet; - packet.seq = seq; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mission_item_reached_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, seq); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); -#endif -#else - mavlink_mission_item_reached_t *packet = (mavlink_mission_item_reached_t *)msgbuf; - packet->seq = seq; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MISSION_ITEM_REACHED UNPACKING - - -/** - * @brief Get field seq from mission_item_reached message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_mission_item_reached_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a mission_item_reached message into a struct - * - * @param msg The message to decode - * @param mission_item_reached C-struct to decode the message contents into - */ -static inline void mavlink_msg_mission_item_reached_decode(const mavlink_message_t* msg, mavlink_mission_item_reached_t* mission_item_reached) -{ -#if MAVLINK_NEED_BYTE_SWAP - mission_item_reached->seq = mavlink_msg_mission_item_reached_get_seq(msg); -#else - memcpy(mission_item_reached, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h deleted file mode 100644 index 29b0ef6ef..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE MISSION_REQUEST PACKING - -#define MAVLINK_MSG_ID_MISSION_REQUEST 40 - -typedef struct __mavlink_mission_request_t -{ - uint16_t seq; ///< Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_mission_request_t; - -#define MAVLINK_MSG_ID_MISSION_REQUEST_LEN 4 -#define MAVLINK_MSG_ID_40_LEN 4 - -#define MAVLINK_MSG_ID_MISSION_REQUEST_CRC 230 -#define MAVLINK_MSG_ID_40_CRC 230 - - - -#define MAVLINK_MESSAGE_INFO_MISSION_REQUEST { \ - "MISSION_REQUEST", \ - 3, \ - { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_request_t, seq) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_request_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a mission_request message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); -#else - mavlink_mission_request_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); -#endif -} - -/** - * @brief Pack a mission_request message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); -#else - mavlink_mission_request_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); -#endif -} - -/** - * @brief Encode a mission_request struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mission_request C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request) -{ - return mavlink_msg_mission_request_pack(system_id, component_id, msg, mission_request->target_system, mission_request->target_component, mission_request->seq); -} - -/** - * @brief Encode a mission_request struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mission_request C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request) -{ - return mavlink_msg_mission_request_pack_chan(system_id, component_id, chan, msg, mission_request->target_system, mission_request->target_component, mission_request->seq); -} - -/** - * @brief Send a mission_request message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); -#endif -#else - mavlink_mission_request_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MISSION_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); -#endif -#else - mavlink_mission_request_t *packet = (mavlink_mission_request_t *)msgbuf; - packet->seq = seq; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MISSION_REQUEST UNPACKING - - -/** - * @brief Get field target_system from mission_request message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mission_request_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from mission_request message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mission_request_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field seq from mission_request message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_mission_request_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a mission_request message into a struct - * - * @param msg The message to decode - * @param mission_request C-struct to decode the message contents into - */ -static inline void mavlink_msg_mission_request_decode(const mavlink_message_t* msg, mavlink_mission_request_t* mission_request) -{ -#if MAVLINK_NEED_BYTE_SWAP - mission_request->seq = mavlink_msg_mission_request_get_seq(msg); - mission_request->target_system = mavlink_msg_mission_request_get_target_system(msg); - mission_request->target_component = mavlink_msg_mission_request_get_target_component(msg); -#else - memcpy(mission_request, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_REQUEST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h deleted file mode 100644 index a275348ac..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE MISSION_REQUEST_LIST PACKING - -#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST 43 - -typedef struct __mavlink_mission_request_list_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_mission_request_list_t; - -#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN 2 -#define MAVLINK_MSG_ID_43_LEN 2 - -#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC 132 -#define MAVLINK_MSG_ID_43_CRC 132 - - - -#define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST { \ - "MISSION_REQUEST_LIST", \ - 2, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_request_list_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_request_list_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a mission_request_list message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); -#else - mavlink_mission_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); -#endif -} - -/** - * @brief Pack a mission_request_list message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); -#else - mavlink_mission_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); -#endif -} - -/** - * @brief Encode a mission_request_list struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mission_request_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) -{ - return mavlink_msg_mission_request_list_pack(system_id, component_id, msg, mission_request_list->target_system, mission_request_list->target_component); -} - -/** - * @brief Encode a mission_request_list struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mission_request_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) -{ - return mavlink_msg_mission_request_list_pack_chan(system_id, component_id, chan, msg, mission_request_list->target_system, mission_request_list->target_component); -} - -/** - * @brief Send a mission_request_list message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); -#endif -#else - mavlink_mission_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mission_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); -#endif -#else - mavlink_mission_request_list_t *packet = (mavlink_mission_request_list_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MISSION_REQUEST_LIST UNPACKING - - -/** - * @brief Get field target_system from mission_request_list message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mission_request_list_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from mission_request_list message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mission_request_list_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a mission_request_list message into a struct - * - * @param msg The message to decode - * @param mission_request_list C-struct to decode the message contents into - */ -static inline void mavlink_msg_mission_request_list_decode(const mavlink_message_t* msg, mavlink_mission_request_list_t* mission_request_list) -{ -#if MAVLINK_NEED_BYTE_SWAP - mission_request_list->target_system = mavlink_msg_mission_request_list_get_target_system(msg); - mission_request_list->target_component = mavlink_msg_mission_request_list_get_target_component(msg); -#else - memcpy(mission_request_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h deleted file mode 100644 index 79a88dc08..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE MISSION_REQUEST_PARTIAL_LIST PACKING - -#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST 37 - -typedef struct __mavlink_mission_request_partial_list_t -{ - int16_t start_index; ///< Start index, 0 by default - int16_t end_index; ///< End index, -1 by default (-1: send list to end). Else a valid index of the list - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_mission_request_partial_list_t; - -#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN 6 -#define MAVLINK_MSG_ID_37_LEN 6 - -#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC 212 -#define MAVLINK_MSG_ID_37_CRC 212 - - - -#define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST { \ - "MISSION_REQUEST_PARTIAL_LIST", \ - 4, \ - { { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_request_partial_list_t, start_index) }, \ - { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_request_partial_list_t, end_index) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_partial_list_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_request_partial_list_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a mission_request_partial_list message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param start_index Start index, 0 by default - * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; - _mav_put_int16_t(buf, 0, start_index); - _mav_put_int16_t(buf, 2, end_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); -#else - mavlink_mission_request_partial_list_t packet; - packet.start_index = start_index; - packet.end_index = end_index; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); -#endif -} - -/** - * @brief Pack a mission_request_partial_list message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param start_index Start index, 0 by default - * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; - _mav_put_int16_t(buf, 0, start_index); - _mav_put_int16_t(buf, 2, end_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); -#else - mavlink_mission_request_partial_list_t packet; - packet.start_index = start_index; - packet.end_index = end_index; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); -#endif -} - -/** - * @brief Encode a mission_request_partial_list struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mission_request_partial_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) -{ - return mavlink_msg_mission_request_partial_list_pack(system_id, component_id, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); -} - -/** - * @brief Encode a mission_request_partial_list struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mission_request_partial_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_request_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) -{ - return mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, chan, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); -} - -/** - * @brief Send a mission_request_partial_list message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param start_index Start index, 0 by default - * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; - _mav_put_int16_t(buf, 0, start_index); - _mav_put_int16_t(buf, 2, end_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); -#endif -#else - mavlink_mission_request_partial_list_t packet; - packet.start_index = start_index; - packet.end_index = end_index; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int16_t(buf, 0, start_index); - _mav_put_int16_t(buf, 2, end_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); -#endif -#else - mavlink_mission_request_partial_list_t *packet = (mavlink_mission_request_partial_list_t *)msgbuf; - packet->start_index = start_index; - packet->end_index = end_index; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MISSION_REQUEST_PARTIAL_LIST UNPACKING - - -/** - * @brief Get field target_system from mission_request_partial_list message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mission_request_partial_list_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field target_component from mission_request_partial_list message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mission_request_partial_list_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field start_index from mission_request_partial_list message - * - * @return Start index, 0 by default - */ -static inline int16_t mavlink_msg_mission_request_partial_list_get_start_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 0); -} - -/** - * @brief Get field end_index from mission_request_partial_list message - * - * @return End index, -1 by default (-1: send list to end). Else a valid index of the list - */ -static inline int16_t mavlink_msg_mission_request_partial_list_get_end_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 2); -} - -/** - * @brief Decode a mission_request_partial_list message into a struct - * - * @param msg The message to decode - * @param mission_request_partial_list C-struct to decode the message contents into - */ -static inline void mavlink_msg_mission_request_partial_list_decode(const mavlink_message_t* msg, mavlink_mission_request_partial_list_t* mission_request_partial_list) -{ -#if MAVLINK_NEED_BYTE_SWAP - mission_request_partial_list->start_index = mavlink_msg_mission_request_partial_list_get_start_index(msg); - mission_request_partial_list->end_index = mavlink_msg_mission_request_partial_list_get_end_index(msg); - mission_request_partial_list->target_system = mavlink_msg_mission_request_partial_list_get_target_system(msg); - mission_request_partial_list->target_component = mavlink_msg_mission_request_partial_list_get_target_component(msg); -#else - memcpy(mission_request_partial_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h deleted file mode 100644 index 0c2a3ada4..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE MISSION_SET_CURRENT PACKING - -#define MAVLINK_MSG_ID_MISSION_SET_CURRENT 41 - -typedef struct __mavlink_mission_set_current_t -{ - uint16_t seq; ///< Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_mission_set_current_t; - -#define MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN 4 -#define MAVLINK_MSG_ID_41_LEN 4 - -#define MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC 28 -#define MAVLINK_MSG_ID_41_CRC 28 - - - -#define MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT { \ - "MISSION_SET_CURRENT", \ - 3, \ - { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_set_current_t, seq) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_set_current_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_set_current_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a mission_set_current message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_set_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); -#else - mavlink_mission_set_current_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_SET_CURRENT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); -#endif -} - -/** - * @brief Pack a mission_set_current message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_set_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); -#else - mavlink_mission_set_current_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_SET_CURRENT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); -#endif -} - -/** - * @brief Encode a mission_set_current struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mission_set_current C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_set_current_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_set_current_t* mission_set_current) -{ - return mavlink_msg_mission_set_current_pack(system_id, component_id, msg, mission_set_current->target_system, mission_set_current->target_component, mission_set_current->seq); -} - -/** - * @brief Encode a mission_set_current struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mission_set_current C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_set_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_set_current_t* mission_set_current) -{ - return mavlink_msg_mission_set_current_pack_chan(system_id, component_id, chan, msg, mission_set_current->target_system, mission_set_current->target_component, mission_set_current->seq); -} - -/** - * @brief Send a mission_set_current message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mission_set_current_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); -#endif -#else - mavlink_mission_set_current_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mission_set_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); -#endif -#else - mavlink_mission_set_current_t *packet = (mavlink_mission_set_current_t *)msgbuf; - packet->seq = seq; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MISSION_SET_CURRENT UNPACKING - - -/** - * @brief Get field target_system from mission_set_current message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mission_set_current_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from mission_set_current message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mission_set_current_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field seq from mission_set_current message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_mission_set_current_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a mission_set_current message into a struct - * - * @param msg The message to decode - * @param mission_set_current C-struct to decode the message contents into - */ -static inline void mavlink_msg_mission_set_current_decode(const mavlink_message_t* msg, mavlink_mission_set_current_t* mission_set_current) -{ -#if MAVLINK_NEED_BYTE_SWAP - mission_set_current->seq = mavlink_msg_mission_set_current_get_seq(msg); - mission_set_current->target_system = mavlink_msg_mission_set_current_get_target_system(msg); - mission_set_current->target_component = mavlink_msg_mission_set_current_get_target_component(msg); -#else - memcpy(mission_set_current, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h deleted file mode 100644 index 17a5a6bdd..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE MISSION_WRITE_PARTIAL_LIST PACKING - -#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST 38 - -typedef struct __mavlink_mission_write_partial_list_t -{ - int16_t start_index; ///< Start index, 0 by default and smaller / equal to the largest index of the current onboard list. - int16_t end_index; ///< End index, equal or greater than start index. - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_mission_write_partial_list_t; - -#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN 6 -#define MAVLINK_MSG_ID_38_LEN 6 - -#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC 9 -#define MAVLINK_MSG_ID_38_CRC 9 - - - -#define MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST { \ - "MISSION_WRITE_PARTIAL_LIST", \ - 4, \ - { { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_write_partial_list_t, start_index) }, \ - { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_write_partial_list_t, end_index) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_write_partial_list_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_write_partial_list_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a mission_write_partial_list message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param start_index Start index, 0 by default and smaller / equal to the largest index of the current onboard list. - * @param end_index End index, equal or greater than start index. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; - _mav_put_int16_t(buf, 0, start_index); - _mav_put_int16_t(buf, 2, end_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); -#else - mavlink_mission_write_partial_list_t packet; - packet.start_index = start_index; - packet.end_index = end_index; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); -#endif -} - -/** - * @brief Pack a mission_write_partial_list message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param start_index Start index, 0 by default and smaller / equal to the largest index of the current onboard list. - * @param end_index End index, equal or greater than start index. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; - _mav_put_int16_t(buf, 0, start_index); - _mav_put_int16_t(buf, 2, end_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); -#else - mavlink_mission_write_partial_list_t packet; - packet.start_index = start_index; - packet.end_index = end_index; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); -#endif -} - -/** - * @brief Encode a mission_write_partial_list struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mission_write_partial_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) -{ - return mavlink_msg_mission_write_partial_list_pack(system_id, component_id, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); -} - -/** - * @brief Encode a mission_write_partial_list struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mission_write_partial_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_write_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) -{ - return mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, chan, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); -} - -/** - * @brief Send a mission_write_partial_list message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param start_index Start index, 0 by default and smaller / equal to the largest index of the current onboard list. - * @param end_index End index, equal or greater than start index. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; - _mav_put_int16_t(buf, 0, start_index); - _mav_put_int16_t(buf, 2, end_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); -#endif -#else - mavlink_mission_write_partial_list_t packet; - packet.start_index = start_index; - packet.end_index = end_index; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int16_t(buf, 0, start_index); - _mav_put_int16_t(buf, 2, end_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); -#endif -#else - mavlink_mission_write_partial_list_t *packet = (mavlink_mission_write_partial_list_t *)msgbuf; - packet->start_index = start_index; - packet->end_index = end_index; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MISSION_WRITE_PARTIAL_LIST UNPACKING - - -/** - * @brief Get field target_system from mission_write_partial_list message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_mission_write_partial_list_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field target_component from mission_write_partial_list message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_mission_write_partial_list_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field start_index from mission_write_partial_list message - * - * @return Start index, 0 by default and smaller / equal to the largest index of the current onboard list. - */ -static inline int16_t mavlink_msg_mission_write_partial_list_get_start_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 0); -} - -/** - * @brief Get field end_index from mission_write_partial_list message - * - * @return End index, equal or greater than start index. - */ -static inline int16_t mavlink_msg_mission_write_partial_list_get_end_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 2); -} - -/** - * @brief Decode a mission_write_partial_list message into a struct - * - * @param msg The message to decode - * @param mission_write_partial_list C-struct to decode the message contents into - */ -static inline void mavlink_msg_mission_write_partial_list_decode(const mavlink_message_t* msg, mavlink_mission_write_partial_list_t* mission_write_partial_list) -{ -#if MAVLINK_NEED_BYTE_SWAP - mission_write_partial_list->start_index = mavlink_msg_mission_write_partial_list_get_start_index(msg); - mission_write_partial_list->end_index = mavlink_msg_mission_write_partial_list_get_end_index(msg); - mission_write_partial_list->target_system = mavlink_msg_mission_write_partial_list_get_target_system(msg); - mission_write_partial_list->target_component = mavlink_msg_mission_write_partial_list_get_target_component(msg); -#else - memcpy(mission_write_partial_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h deleted file mode 100644 index df82704a4..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h +++ /dev/null @@ -1,249 +0,0 @@ -// MESSAGE NAMED_VALUE_FLOAT PACKING - -#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT 251 - -typedef struct __mavlink_named_value_float_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - float value; ///< Floating point value - char name[10]; ///< Name of the debug variable -} mavlink_named_value_float_t; - -#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN 18 -#define MAVLINK_MSG_ID_251_LEN 18 - -#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC 170 -#define MAVLINK_MSG_ID_251_CRC 170 - -#define MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10 - -#define MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT { \ - "NAMED_VALUE_FLOAT", \ - 3, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_named_value_float_t, time_boot_ms) }, \ - { "value", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_named_value_float_t, value) }, \ - { "name", NULL, MAVLINK_TYPE_CHAR, 10, 8, offsetof(mavlink_named_value_float_t, name) }, \ - } \ -} - - -/** - * @brief Pack a named_value_float message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param name Name of the debug variable - * @param value Floating point value - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, const char *name, float value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, value); - _mav_put_char_array(buf, 8, name, 10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); -#else - mavlink_named_value_float_t packet; - packet.time_boot_ms = time_boot_ms; - packet.value = value; - mav_array_memcpy(packet.name, name, sizeof(char)*10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); -#endif -} - -/** - * @brief Pack a named_value_float message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param name Name of the debug variable - * @param value Floating point value - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,const char *name,float value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, value); - _mav_put_char_array(buf, 8, name, 10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); -#else - mavlink_named_value_float_t packet; - packet.time_boot_ms = time_boot_ms; - packet.value = value; - mav_array_memcpy(packet.name, name, sizeof(char)*10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); -#endif -} - -/** - * @brief Encode a named_value_float struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param named_value_float C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_named_value_float_t* named_value_float) -{ - return mavlink_msg_named_value_float_pack(system_id, component_id, msg, named_value_float->time_boot_ms, named_value_float->name, named_value_float->value); -} - -/** - * @brief Encode a named_value_float struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param named_value_float C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_named_value_float_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_named_value_float_t* named_value_float) -{ - return mavlink_msg_named_value_float_pack_chan(system_id, component_id, chan, msg, named_value_float->time_boot_ms, named_value_float->name, named_value_float->value); -} - -/** - * @brief Send a named_value_float message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param name Name of the debug variable - * @param value Floating point value - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, float value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, value); - _mav_put_char_array(buf, 8, name, 10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); -#endif -#else - mavlink_named_value_float_t packet; - packet.time_boot_ms = time_boot_ms; - packet.value = value; - mav_array_memcpy(packet.name, name, sizeof(char)*10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)&packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)&packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_named_value_float_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, float value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, value); - _mav_put_char_array(buf, 8, name, 10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); -#endif -#else - mavlink_named_value_float_t *packet = (mavlink_named_value_float_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->value = value; - mav_array_memcpy(packet->name, name, sizeof(char)*10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE NAMED_VALUE_FLOAT UNPACKING - - -/** - * @brief Get field time_boot_ms from named_value_float message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_named_value_float_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field name from named_value_float message - * - * @return Name of the debug variable - */ -static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_message_t* msg, char *name) -{ - return _MAV_RETURN_char_array(msg, name, 10, 8); -} - -/** - * @brief Get field value from named_value_float message - * - * @return Floating point value - */ -static inline float mavlink_msg_named_value_float_get_value(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Decode a named_value_float message into a struct - * - * @param msg The message to decode - * @param named_value_float C-struct to decode the message contents into - */ -static inline void mavlink_msg_named_value_float_decode(const mavlink_message_t* msg, mavlink_named_value_float_t* named_value_float) -{ -#if MAVLINK_NEED_BYTE_SWAP - named_value_float->time_boot_ms = mavlink_msg_named_value_float_get_time_boot_ms(msg); - named_value_float->value = mavlink_msg_named_value_float_get_value(msg); - mavlink_msg_named_value_float_get_name(msg, named_value_float->name); -#else - memcpy(named_value_float, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h deleted file mode 100644 index cfdd73d61..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h +++ /dev/null @@ -1,249 +0,0 @@ -// MESSAGE NAMED_VALUE_INT PACKING - -#define MAVLINK_MSG_ID_NAMED_VALUE_INT 252 - -typedef struct __mavlink_named_value_int_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - int32_t value; ///< Signed integer value - char name[10]; ///< Name of the debug variable -} mavlink_named_value_int_t; - -#define MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN 18 -#define MAVLINK_MSG_ID_252_LEN 18 - -#define MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC 44 -#define MAVLINK_MSG_ID_252_CRC 44 - -#define MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN 10 - -#define MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT { \ - "NAMED_VALUE_INT", \ - 3, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_named_value_int_t, time_boot_ms) }, \ - { "value", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_named_value_int_t, value) }, \ - { "name", NULL, MAVLINK_TYPE_CHAR, 10, 8, offsetof(mavlink_named_value_int_t, name) }, \ - } \ -} - - -/** - * @brief Pack a named_value_int message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param name Name of the debug variable - * @param value Signed integer value - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, const char *name, int32_t value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, value); - _mav_put_char_array(buf, 8, name, 10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); -#else - mavlink_named_value_int_t packet; - packet.time_boot_ms = time_boot_ms; - packet.value = value; - mav_array_memcpy(packet.name, name, sizeof(char)*10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); -#endif -} - -/** - * @brief Pack a named_value_int message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param name Name of the debug variable - * @param value Signed integer value - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,const char *name,int32_t value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, value); - _mav_put_char_array(buf, 8, name, 10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); -#else - mavlink_named_value_int_t packet; - packet.time_boot_ms = time_boot_ms; - packet.value = value; - mav_array_memcpy(packet.name, name, sizeof(char)*10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); -#endif -} - -/** - * @brief Encode a named_value_int struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param named_value_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_named_value_int_t* named_value_int) -{ - return mavlink_msg_named_value_int_pack(system_id, component_id, msg, named_value_int->time_boot_ms, named_value_int->name, named_value_int->value); -} - -/** - * @brief Encode a named_value_int struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param named_value_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_named_value_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_named_value_int_t* named_value_int) -{ - return mavlink_msg_named_value_int_pack_chan(system_id, component_id, chan, msg, named_value_int->time_boot_ms, named_value_int->name, named_value_int->value); -} - -/** - * @brief Send a named_value_int message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param name Name of the debug variable - * @param value Signed integer value - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, int32_t value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, value); - _mav_put_char_array(buf, 8, name, 10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); -#endif -#else - mavlink_named_value_int_t packet; - packet.time_boot_ms = time_boot_ms; - packet.value = value; - mav_array_memcpy(packet.name, name, sizeof(char)*10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)&packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)&packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_named_value_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, int32_t value) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, value); - _mav_put_char_array(buf, 8, name, 10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); -#endif -#else - mavlink_named_value_int_t *packet = (mavlink_named_value_int_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->value = value; - mav_array_memcpy(packet->name, name, sizeof(char)*10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE NAMED_VALUE_INT UNPACKING - - -/** - * @brief Get field time_boot_ms from named_value_int message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_named_value_int_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field name from named_value_int message - * - * @return Name of the debug variable - */ -static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_message_t* msg, char *name) -{ - return _MAV_RETURN_char_array(msg, name, 10, 8); -} - -/** - * @brief Get field value from named_value_int message - * - * @return Signed integer value - */ -static inline int32_t mavlink_msg_named_value_int_get_value(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Decode a named_value_int message into a struct - * - * @param msg The message to decode - * @param named_value_int C-struct to decode the message contents into - */ -static inline void mavlink_msg_named_value_int_decode(const mavlink_message_t* msg, mavlink_named_value_int_t* named_value_int) -{ -#if MAVLINK_NEED_BYTE_SWAP - named_value_int->time_boot_ms = mavlink_msg_named_value_int_get_time_boot_ms(msg); - named_value_int->value = mavlink_msg_named_value_int_get_value(msg); - mavlink_msg_named_value_int_get_name(msg, named_value_int->name); -#else - memcpy(named_value_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h deleted file mode 100644 index b9a748b22..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h +++ /dev/null @@ -1,377 +0,0 @@ -// MESSAGE NAV_CONTROLLER_OUTPUT PACKING - -#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT 62 - -typedef struct __mavlink_nav_controller_output_t -{ - float nav_roll; ///< Current desired roll in degrees - float nav_pitch; ///< Current desired pitch in degrees - float alt_error; ///< Current altitude error in meters - float aspd_error; ///< Current airspeed error in meters/second - float xtrack_error; ///< Current crosstrack error on x-y plane in meters - int16_t nav_bearing; ///< Current desired heading in degrees - int16_t target_bearing; ///< Bearing to current MISSION/target in degrees - uint16_t wp_dist; ///< Distance to active MISSION in meters -} mavlink_nav_controller_output_t; - -#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN 26 -#define MAVLINK_MSG_ID_62_LEN 26 - -#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC 183 -#define MAVLINK_MSG_ID_62_CRC 183 - - - -#define MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT { \ - "NAV_CONTROLLER_OUTPUT", \ - 8, \ - { { "nav_roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_nav_controller_output_t, nav_roll) }, \ - { "nav_pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_nav_controller_output_t, nav_pitch) }, \ - { "alt_error", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_nav_controller_output_t, alt_error) }, \ - { "aspd_error", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_nav_controller_output_t, aspd_error) }, \ - { "xtrack_error", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_nav_controller_output_t, xtrack_error) }, \ - { "nav_bearing", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_nav_controller_output_t, nav_bearing) }, \ - { "target_bearing", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_nav_controller_output_t, target_bearing) }, \ - { "wp_dist", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_nav_controller_output_t, wp_dist) }, \ - } \ -} - - -/** - * @brief Pack a nav_controller_output message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param nav_roll Current desired roll in degrees - * @param nav_pitch Current desired pitch in degrees - * @param nav_bearing Current desired heading in degrees - * @param target_bearing Bearing to current MISSION/target in degrees - * @param wp_dist Distance to active MISSION in meters - * @param alt_error Current altitude error in meters - * @param aspd_error Current airspeed error in meters/second - * @param xtrack_error Current crosstrack error on x-y plane in meters - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN]; - _mav_put_float(buf, 0, nav_roll); - _mav_put_float(buf, 4, nav_pitch); - _mav_put_float(buf, 8, alt_error); - _mav_put_float(buf, 12, aspd_error); - _mav_put_float(buf, 16, xtrack_error); - _mav_put_int16_t(buf, 20, nav_bearing); - _mav_put_int16_t(buf, 22, target_bearing); - _mav_put_uint16_t(buf, 24, wp_dist); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); -#else - mavlink_nav_controller_output_t packet; - packet.nav_roll = nav_roll; - packet.nav_pitch = nav_pitch; - packet.alt_error = alt_error; - packet.aspd_error = aspd_error; - packet.xtrack_error = xtrack_error; - packet.nav_bearing = nav_bearing; - packet.target_bearing = target_bearing; - packet.wp_dist = wp_dist; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); -#endif -} - -/** - * @brief Pack a nav_controller_output message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param nav_roll Current desired roll in degrees - * @param nav_pitch Current desired pitch in degrees - * @param nav_bearing Current desired heading in degrees - * @param target_bearing Bearing to current MISSION/target in degrees - * @param wp_dist Distance to active MISSION in meters - * @param alt_error Current altitude error in meters - * @param aspd_error Current airspeed error in meters/second - * @param xtrack_error Current crosstrack error on x-y plane in meters - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float nav_roll,float nav_pitch,int16_t nav_bearing,int16_t target_bearing,uint16_t wp_dist,float alt_error,float aspd_error,float xtrack_error) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN]; - _mav_put_float(buf, 0, nav_roll); - _mav_put_float(buf, 4, nav_pitch); - _mav_put_float(buf, 8, alt_error); - _mav_put_float(buf, 12, aspd_error); - _mav_put_float(buf, 16, xtrack_error); - _mav_put_int16_t(buf, 20, nav_bearing); - _mav_put_int16_t(buf, 22, target_bearing); - _mav_put_uint16_t(buf, 24, wp_dist); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); -#else - mavlink_nav_controller_output_t packet; - packet.nav_roll = nav_roll; - packet.nav_pitch = nav_pitch; - packet.alt_error = alt_error; - packet.aspd_error = aspd_error; - packet.xtrack_error = xtrack_error; - packet.nav_bearing = nav_bearing; - packet.target_bearing = target_bearing; - packet.wp_dist = wp_dist; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); -#endif -} - -/** - * @brief Encode a nav_controller_output struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param nav_controller_output C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_nav_controller_output_t* nav_controller_output) -{ - return mavlink_msg_nav_controller_output_pack(system_id, component_id, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); -} - -/** - * @brief Encode a nav_controller_output struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param nav_controller_output C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_nav_controller_output_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_nav_controller_output_t* nav_controller_output) -{ - return mavlink_msg_nav_controller_output_pack_chan(system_id, component_id, chan, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); -} - -/** - * @brief Send a nav_controller_output message - * @param chan MAVLink channel to send the message - * - * @param nav_roll Current desired roll in degrees - * @param nav_pitch Current desired pitch in degrees - * @param nav_bearing Current desired heading in degrees - * @param target_bearing Bearing to current MISSION/target in degrees - * @param wp_dist Distance to active MISSION in meters - * @param alt_error Current altitude error in meters - * @param aspd_error Current airspeed error in meters/second - * @param xtrack_error Current crosstrack error on x-y plane in meters - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN]; - _mav_put_float(buf, 0, nav_roll); - _mav_put_float(buf, 4, nav_pitch); - _mav_put_float(buf, 8, alt_error); - _mav_put_float(buf, 12, aspd_error); - _mav_put_float(buf, 16, xtrack_error); - _mav_put_int16_t(buf, 20, nav_bearing); - _mav_put_int16_t(buf, 22, target_bearing); - _mav_put_uint16_t(buf, 24, wp_dist); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); -#endif -#else - mavlink_nav_controller_output_t packet; - packet.nav_roll = nav_roll; - packet.nav_pitch = nav_pitch; - packet.alt_error = alt_error; - packet.aspd_error = aspd_error; - packet.xtrack_error = xtrack_error; - packet.nav_bearing = nav_bearing; - packet.target_bearing = target_bearing; - packet.wp_dist = wp_dist; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)&packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)&packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_nav_controller_output_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, nav_roll); - _mav_put_float(buf, 4, nav_pitch); - _mav_put_float(buf, 8, alt_error); - _mav_put_float(buf, 12, aspd_error); - _mav_put_float(buf, 16, xtrack_error); - _mav_put_int16_t(buf, 20, nav_bearing); - _mav_put_int16_t(buf, 22, target_bearing); - _mav_put_uint16_t(buf, 24, wp_dist); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); -#endif -#else - mavlink_nav_controller_output_t *packet = (mavlink_nav_controller_output_t *)msgbuf; - packet->nav_roll = nav_roll; - packet->nav_pitch = nav_pitch; - packet->alt_error = alt_error; - packet->aspd_error = aspd_error; - packet->xtrack_error = xtrack_error; - packet->nav_bearing = nav_bearing; - packet->target_bearing = target_bearing; - packet->wp_dist = wp_dist; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE NAV_CONTROLLER_OUTPUT UNPACKING - - -/** - * @brief Get field nav_roll from nav_controller_output message - * - * @return Current desired roll in degrees - */ -static inline float mavlink_msg_nav_controller_output_get_nav_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field nav_pitch from nav_controller_output message - * - * @return Current desired pitch in degrees - */ -static inline float mavlink_msg_nav_controller_output_get_nav_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field nav_bearing from nav_controller_output message - * - * @return Current desired heading in degrees - */ -static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Get field target_bearing from nav_controller_output message - * - * @return Bearing to current MISSION/target in degrees - */ -static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 22); -} - -/** - * @brief Get field wp_dist from nav_controller_output message - * - * @return Distance to active MISSION in meters - */ -static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 24); -} - -/** - * @brief Get field alt_error from nav_controller_output message - * - * @return Current altitude error in meters - */ -static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field aspd_error from nav_controller_output message - * - * @return Current airspeed error in meters/second - */ -static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field xtrack_error from nav_controller_output message - * - * @return Current crosstrack error on x-y plane in meters - */ -static inline float mavlink_msg_nav_controller_output_get_xtrack_error(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a nav_controller_output message into a struct - * - * @param msg The message to decode - * @param nav_controller_output C-struct to decode the message contents into - */ -static inline void mavlink_msg_nav_controller_output_decode(const mavlink_message_t* msg, mavlink_nav_controller_output_t* nav_controller_output) -{ -#if MAVLINK_NEED_BYTE_SWAP - nav_controller_output->nav_roll = mavlink_msg_nav_controller_output_get_nav_roll(msg); - nav_controller_output->nav_pitch = mavlink_msg_nav_controller_output_get_nav_pitch(msg); - nav_controller_output->alt_error = mavlink_msg_nav_controller_output_get_alt_error(msg); - nav_controller_output->aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(msg); - nav_controller_output->xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(msg); - nav_controller_output->nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(msg); - nav_controller_output->target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(msg); - nav_controller_output->wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(msg); -#else - memcpy(nav_controller_output, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h deleted file mode 100644 index 4ee9c452f..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h +++ /dev/null @@ -1,322 +0,0 @@ -// MESSAGE OMNIDIRECTIONAL_FLOW PACKING - -#define MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW 106 - -typedef struct __mavlink_omnidirectional_flow_t -{ - uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) - float front_distance_m; ///< Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance - int16_t left[10]; ///< Flow in deci pixels (1 = 0.1 pixel) on left hemisphere - int16_t right[10]; ///< Flow in deci pixels (1 = 0.1 pixel) on right hemisphere - uint8_t sensor_id; ///< Sensor ID - uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality -} mavlink_omnidirectional_flow_t; - -#define MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN 54 -#define MAVLINK_MSG_ID_106_LEN 54 - -#define MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC 211 -#define MAVLINK_MSG_ID_106_CRC 211 - -#define MAVLINK_MSG_OMNIDIRECTIONAL_FLOW_FIELD_LEFT_LEN 10 -#define MAVLINK_MSG_OMNIDIRECTIONAL_FLOW_FIELD_RIGHT_LEN 10 - -#define MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW { \ - "OMNIDIRECTIONAL_FLOW", \ - 6, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_omnidirectional_flow_t, time_usec) }, \ - { "front_distance_m", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_omnidirectional_flow_t, front_distance_m) }, \ - { "left", NULL, MAVLINK_TYPE_INT16_T, 10, 12, offsetof(mavlink_omnidirectional_flow_t, left) }, \ - { "right", NULL, MAVLINK_TYPE_INT16_T, 10, 32, offsetof(mavlink_omnidirectional_flow_t, right) }, \ - { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_omnidirectional_flow_t, sensor_id) }, \ - { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_omnidirectional_flow_t, quality) }, \ - } \ -} - - -/** - * @brief Pack a omnidirectional_flow message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param sensor_id Sensor ID - * @param left Flow in deci pixels (1 = 0.1 pixel) on left hemisphere - * @param right Flow in deci pixels (1 = 0.1 pixel) on right hemisphere - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param front_distance_m Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_omnidirectional_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t sensor_id, const int16_t *left, const int16_t *right, uint8_t quality, float front_distance_m) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, front_distance_m); - _mav_put_uint8_t(buf, 52, sensor_id); - _mav_put_uint8_t(buf, 53, quality); - _mav_put_int16_t_array(buf, 12, left, 10); - _mav_put_int16_t_array(buf, 32, right, 10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#else - mavlink_omnidirectional_flow_t packet; - packet.time_usec = time_usec; - packet.front_distance_m = front_distance_m; - packet.sensor_id = sensor_id; - packet.quality = quality; - mav_array_memcpy(packet.left, left, sizeof(int16_t)*10); - mav_array_memcpy(packet.right, right, sizeof(int16_t)*10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif -} - -/** - * @brief Pack a omnidirectional_flow message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param sensor_id Sensor ID - * @param left Flow in deci pixels (1 = 0.1 pixel) on left hemisphere - * @param right Flow in deci pixels (1 = 0.1 pixel) on right hemisphere - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param front_distance_m Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_omnidirectional_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,uint8_t sensor_id,const int16_t *left,const int16_t *right,uint8_t quality,float front_distance_m) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, front_distance_m); - _mav_put_uint8_t(buf, 52, sensor_id); - _mav_put_uint8_t(buf, 53, quality); - _mav_put_int16_t_array(buf, 12, left, 10); - _mav_put_int16_t_array(buf, 32, right, 10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#else - mavlink_omnidirectional_flow_t packet; - packet.time_usec = time_usec; - packet.front_distance_m = front_distance_m; - packet.sensor_id = sensor_id; - packet.quality = quality; - mav_array_memcpy(packet.left, left, sizeof(int16_t)*10); - mav_array_memcpy(packet.right, right, sizeof(int16_t)*10); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif -} - -/** - * @brief Encode a omnidirectional_flow struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param omnidirectional_flow C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_omnidirectional_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_omnidirectional_flow_t* omnidirectional_flow) -{ - return mavlink_msg_omnidirectional_flow_pack(system_id, component_id, msg, omnidirectional_flow->time_usec, omnidirectional_flow->sensor_id, omnidirectional_flow->left, omnidirectional_flow->right, omnidirectional_flow->quality, omnidirectional_flow->front_distance_m); -} - -/** - * @brief Encode a omnidirectional_flow struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param omnidirectional_flow C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_omnidirectional_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_omnidirectional_flow_t* omnidirectional_flow) -{ - return mavlink_msg_omnidirectional_flow_pack_chan(system_id, component_id, chan, msg, omnidirectional_flow->time_usec, omnidirectional_flow->sensor_id, omnidirectional_flow->left, omnidirectional_flow->right, omnidirectional_flow->quality, omnidirectional_flow->front_distance_m); -} - -/** - * @brief Send a omnidirectional_flow message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param sensor_id Sensor ID - * @param left Flow in deci pixels (1 = 0.1 pixel) on left hemisphere - * @param right Flow in deci pixels (1 = 0.1 pixel) on right hemisphere - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param front_distance_m Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_omnidirectional_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, const int16_t *left, const int16_t *right, uint8_t quality, float front_distance_m) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, front_distance_m); - _mav_put_uint8_t(buf, 52, sensor_id); - _mav_put_uint8_t(buf, 53, quality); - _mav_put_int16_t_array(buf, 12, left, 10); - _mav_put_int16_t_array(buf, 32, right, 10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif -#else - mavlink_omnidirectional_flow_t packet; - packet.time_usec = time_usec; - packet.front_distance_m = front_distance_m; - packet.sensor_id = sensor_id; - packet.quality = quality; - mav_array_memcpy(packet.left, left, sizeof(int16_t)*10); - mav_array_memcpy(packet.right, right, sizeof(int16_t)*10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_omnidirectional_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, const int16_t *left, const int16_t *right, uint8_t quality, float front_distance_m) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, front_distance_m); - _mav_put_uint8_t(buf, 52, sensor_id); - _mav_put_uint8_t(buf, 53, quality); - _mav_put_int16_t_array(buf, 12, left, 10); - _mav_put_int16_t_array(buf, 32, right, 10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif -#else - mavlink_omnidirectional_flow_t *packet = (mavlink_omnidirectional_flow_t *)msgbuf; - packet->time_usec = time_usec; - packet->front_distance_m = front_distance_m; - packet->sensor_id = sensor_id; - packet->quality = quality; - mav_array_memcpy(packet->left, left, sizeof(int16_t)*10); - mav_array_memcpy(packet->right, right, sizeof(int16_t)*10); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE OMNIDIRECTIONAL_FLOW UNPACKING - - -/** - * @brief Get field time_usec from omnidirectional_flow message - * - * @return Timestamp (microseconds, synced to UNIX time or since system boot) - */ -static inline uint64_t mavlink_msg_omnidirectional_flow_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field sensor_id from omnidirectional_flow message - * - * @return Sensor ID - */ -static inline uint8_t mavlink_msg_omnidirectional_flow_get_sensor_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 52); -} - -/** - * @brief Get field left from omnidirectional_flow message - * - * @return Flow in deci pixels (1 = 0.1 pixel) on left hemisphere - */ -static inline uint16_t mavlink_msg_omnidirectional_flow_get_left(const mavlink_message_t* msg, int16_t *left) -{ - return _MAV_RETURN_int16_t_array(msg, left, 10, 12); -} - -/** - * @brief Get field right from omnidirectional_flow message - * - * @return Flow in deci pixels (1 = 0.1 pixel) on right hemisphere - */ -static inline uint16_t mavlink_msg_omnidirectional_flow_get_right(const mavlink_message_t* msg, int16_t *right) -{ - return _MAV_RETURN_int16_t_array(msg, right, 10, 32); -} - -/** - * @brief Get field quality from omnidirectional_flow message - * - * @return Optical flow quality / confidence. 0: bad, 255: maximum quality - */ -static inline uint8_t mavlink_msg_omnidirectional_flow_get_quality(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 53); -} - -/** - * @brief Get field front_distance_m from omnidirectional_flow message - * - * @return Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance - */ -static inline float mavlink_msg_omnidirectional_flow_get_front_distance_m(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Decode a omnidirectional_flow message into a struct - * - * @param msg The message to decode - * @param omnidirectional_flow C-struct to decode the message contents into - */ -static inline void mavlink_msg_omnidirectional_flow_decode(const mavlink_message_t* msg, mavlink_omnidirectional_flow_t* omnidirectional_flow) -{ -#if MAVLINK_NEED_BYTE_SWAP - omnidirectional_flow->time_usec = mavlink_msg_omnidirectional_flow_get_time_usec(msg); - omnidirectional_flow->front_distance_m = mavlink_msg_omnidirectional_flow_get_front_distance_m(msg); - mavlink_msg_omnidirectional_flow_get_left(msg, omnidirectional_flow->left); - mavlink_msg_omnidirectional_flow_get_right(msg, omnidirectional_flow->right); - omnidirectional_flow->sensor_id = mavlink_msg_omnidirectional_flow_get_sensor_id(msg); - omnidirectional_flow->quality = mavlink_msg_omnidirectional_flow_get_quality(msg); -#else - memcpy(omnidirectional_flow, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h deleted file mode 100644 index 6a2efc664..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h +++ /dev/null @@ -1,377 +0,0 @@ -// MESSAGE OPTICAL_FLOW PACKING - -#define MAVLINK_MSG_ID_OPTICAL_FLOW 100 - -typedef struct __mavlink_optical_flow_t -{ - uint64_t time_usec; ///< Timestamp (UNIX) - float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated - float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated - float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - int16_t flow_x; ///< Flow in pixels * 10 in x-sensor direction (dezi-pixels) - int16_t flow_y; ///< Flow in pixels * 10 in y-sensor direction (dezi-pixels) - uint8_t sensor_id; ///< Sensor ID - uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality -} mavlink_optical_flow_t; - -#define MAVLINK_MSG_ID_OPTICAL_FLOW_LEN 26 -#define MAVLINK_MSG_ID_100_LEN 26 - -#define MAVLINK_MSG_ID_OPTICAL_FLOW_CRC 175 -#define MAVLINK_MSG_ID_100_CRC 175 - - - -#define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW { \ - "OPTICAL_FLOW", \ - 8, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time_usec) }, \ - { "flow_comp_m_x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_optical_flow_t, flow_comp_m_x) }, \ - { "flow_comp_m_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_optical_flow_t, flow_comp_m_y) }, \ - { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_optical_flow_t, ground_distance) }, \ - { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_optical_flow_t, flow_x) }, \ - { "flow_y", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_optical_flow_t, flow_y) }, \ - { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_optical_flow_t, sensor_id) }, \ - { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_optical_flow_t, quality) }, \ - } \ -} - - -/** - * @brief Pack a optical_flow message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (UNIX) - * @param sensor_id Sensor ID - * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) - * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) - * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated - * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, flow_comp_m_x); - _mav_put_float(buf, 12, flow_comp_m_y); - _mav_put_float(buf, 16, ground_distance); - _mav_put_int16_t(buf, 20, flow_x); - _mav_put_int16_t(buf, 22, flow_y); - _mav_put_uint8_t(buf, 24, sensor_id); - _mav_put_uint8_t(buf, 25, quality); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); -#else - mavlink_optical_flow_t packet; - packet.time_usec = time_usec; - packet.flow_comp_m_x = flow_comp_m_x; - packet.flow_comp_m_y = flow_comp_m_y; - packet.ground_distance = ground_distance; - packet.flow_x = flow_x; - packet.flow_y = flow_y; - packet.sensor_id = sensor_id; - packet.quality = quality; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); -#endif -} - -/** - * @brief Pack a optical_flow message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (UNIX) - * @param sensor_id Sensor ID - * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) - * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) - * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated - * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,float flow_comp_m_x,float flow_comp_m_y,uint8_t quality,float ground_distance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, flow_comp_m_x); - _mav_put_float(buf, 12, flow_comp_m_y); - _mav_put_float(buf, 16, ground_distance); - _mav_put_int16_t(buf, 20, flow_x); - _mav_put_int16_t(buf, 22, flow_y); - _mav_put_uint8_t(buf, 24, sensor_id); - _mav_put_uint8_t(buf, 25, quality); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); -#else - mavlink_optical_flow_t packet; - packet.time_usec = time_usec; - packet.flow_comp_m_x = flow_comp_m_x; - packet.flow_comp_m_y = flow_comp_m_y; - packet.ground_distance = ground_distance; - packet.flow_x = flow_x; - packet.flow_y = flow_y; - packet.sensor_id = sensor_id; - packet.quality = quality; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); -#endif -} - -/** - * @brief Encode a optical_flow struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param optical_flow C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) -{ - return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); -} - -/** - * @brief Encode a optical_flow struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param optical_flow C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) -{ - return mavlink_msg_optical_flow_pack_chan(system_id, component_id, chan, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); -} - -/** - * @brief Send a optical_flow message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (UNIX) - * @param sensor_id Sensor ID - * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) - * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) - * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated - * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, flow_comp_m_x); - _mav_put_float(buf, 12, flow_comp_m_y); - _mav_put_float(buf, 16, ground_distance); - _mav_put_int16_t(buf, 20, flow_x); - _mav_put_int16_t(buf, 22, flow_y); - _mav_put_uint8_t(buf, 24, sensor_id); - _mav_put_uint8_t(buf, 25, quality); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); -#endif -#else - mavlink_optical_flow_t packet; - packet.time_usec = time_usec; - packet.flow_comp_m_x = flow_comp_m_x; - packet.flow_comp_m_y = flow_comp_m_y; - packet.ground_distance = ground_distance; - packet.flow_x = flow_x; - packet.flow_y = flow_y; - packet.sensor_id = sensor_id; - packet.quality = quality; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_float(buf, 8, flow_comp_m_x); - _mav_put_float(buf, 12, flow_comp_m_y); - _mav_put_float(buf, 16, ground_distance); - _mav_put_int16_t(buf, 20, flow_x); - _mav_put_int16_t(buf, 22, flow_y); - _mav_put_uint8_t(buf, 24, sensor_id); - _mav_put_uint8_t(buf, 25, quality); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); -#endif -#else - mavlink_optical_flow_t *packet = (mavlink_optical_flow_t *)msgbuf; - packet->time_usec = time_usec; - packet->flow_comp_m_x = flow_comp_m_x; - packet->flow_comp_m_y = flow_comp_m_y; - packet->ground_distance = ground_distance; - packet->flow_x = flow_x; - packet->flow_y = flow_y; - packet->sensor_id = sensor_id; - packet->quality = quality; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE OPTICAL_FLOW UNPACKING - - -/** - * @brief Get field time_usec from optical_flow message - * - * @return Timestamp (UNIX) - */ -static inline uint64_t mavlink_msg_optical_flow_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field sensor_id from optical_flow message - * - * @return Sensor ID - */ -static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 24); -} - -/** - * @brief Get field flow_x from optical_flow message - * - * @return Flow in pixels * 10 in x-sensor direction (dezi-pixels) - */ -static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Get field flow_y from optical_flow message - * - * @return Flow in pixels * 10 in y-sensor direction (dezi-pixels) - */ -static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 22); -} - -/** - * @brief Get field flow_comp_m_x from optical_flow message - * - * @return Flow in meters in x-sensor direction, angular-speed compensated - */ -static inline float mavlink_msg_optical_flow_get_flow_comp_m_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field flow_comp_m_y from optical_flow message - * - * @return Flow in meters in y-sensor direction, angular-speed compensated - */ -static inline float mavlink_msg_optical_flow_get_flow_comp_m_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field quality from optical_flow message - * - * @return Optical flow quality / confidence. 0: bad, 255: maximum quality - */ -static inline uint8_t mavlink_msg_optical_flow_get_quality(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 25); -} - -/** - * @brief Get field ground_distance from optical_flow message - * - * @return Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - */ -static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a optical_flow message into a struct - * - * @param msg The message to decode - * @param optical_flow C-struct to decode the message contents into - */ -static inline void mavlink_msg_optical_flow_decode(const mavlink_message_t* msg, mavlink_optical_flow_t* optical_flow) -{ -#if MAVLINK_NEED_BYTE_SWAP - optical_flow->time_usec = mavlink_msg_optical_flow_get_time_usec(msg); - optical_flow->flow_comp_m_x = mavlink_msg_optical_flow_get_flow_comp_m_x(msg); - optical_flow->flow_comp_m_y = mavlink_msg_optical_flow_get_flow_comp_m_y(msg); - optical_flow->ground_distance = mavlink_msg_optical_flow_get_ground_distance(msg); - optical_flow->flow_x = mavlink_msg_optical_flow_get_flow_x(msg); - optical_flow->flow_y = mavlink_msg_optical_flow_get_flow_y(msg); - optical_flow->sensor_id = mavlink_msg_optical_flow_get_sensor_id(msg); - optical_flow->quality = mavlink_msg_optical_flow_get_quality(msg); -#else - memcpy(optical_flow, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h deleted file mode 100644 index f9466b002..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE PARAM_REQUEST_LIST PACKING - -#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST 21 - -typedef struct __mavlink_param_request_list_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_param_request_list_t; - -#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN 2 -#define MAVLINK_MSG_ID_21_LEN 2 - -#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC 159 -#define MAVLINK_MSG_ID_21_CRC 159 - - - -#define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST { \ - "PARAM_REQUEST_LIST", \ - 2, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_request_list_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_request_list_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a param_request_list message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#else - mavlink_param_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif -} - -/** - * @brief Pack a param_request_list message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#else - mavlink_param_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif -} - -/** - * @brief Encode a param_request_list struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param param_request_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) -{ - return mavlink_msg_param_request_list_pack(system_id, component_id, msg, param_request_list->target_system, param_request_list->target_component); -} - -/** - * @brief Encode a param_request_list struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param param_request_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_param_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) -{ - return mavlink_msg_param_request_list_pack_chan(system_id, component_id, chan, msg, param_request_list->target_system, param_request_list->target_component); -} - -/** - * @brief Send a param_request_list message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif -#else - mavlink_param_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_param_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif -#else - mavlink_param_request_list_t *packet = (mavlink_param_request_list_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE PARAM_REQUEST_LIST UNPACKING - - -/** - * @brief Get field target_system from param_request_list message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from param_request_list message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_param_request_list_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a param_request_list message into a struct - * - * @param msg The message to decode - * @param param_request_list C-struct to decode the message contents into - */ -static inline void mavlink_msg_param_request_list_decode(const mavlink_message_t* msg, mavlink_param_request_list_t* param_request_list) -{ -#if MAVLINK_NEED_BYTE_SWAP - param_request_list->target_system = mavlink_msg_param_request_list_get_target_system(msg); - param_request_list->target_component = mavlink_msg_param_request_list_get_target_component(msg); -#else - memcpy(param_request_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h deleted file mode 100644 index 730cff066..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h +++ /dev/null @@ -1,273 +0,0 @@ -// MESSAGE PARAM_REQUEST_READ PACKING - -#define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20 - -typedef struct __mavlink_param_request_read_t -{ - int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string -} mavlink_param_request_read_t; - -#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 20 -#define MAVLINK_MSG_ID_20_LEN 20 - -#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC 214 -#define MAVLINK_MSG_ID_20_CRC 214 - -#define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 16 - -#define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ { \ - "PARAM_REQUEST_READ", \ - 4, \ - { { "param_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_param_request_read_t, param_index) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_param_request_read_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_param_request_read_t, target_component) }, \ - { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_request_read_t, param_id) }, \ - } \ -} - - -/** - * @brief Pack a param_request_read message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN]; - _mav_put_int16_t(buf, 0, param_index); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_char_array(buf, 4, param_id, 16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#else - mavlink_param_request_read_t packet; - packet.param_index = param_index; - packet.target_system = target_system; - packet.target_component = target_component; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif -} - -/** - * @brief Pack a param_request_read message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,const char *param_id,int16_t param_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN]; - _mav_put_int16_t(buf, 0, param_index); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_char_array(buf, 4, param_id, 16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#else - mavlink_param_request_read_t packet; - packet.param_index = param_index; - packet.target_system = target_system; - packet.target_component = target_component; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif -} - -/** - * @brief Encode a param_request_read struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param param_request_read C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read) -{ - return mavlink_msg_param_request_read_pack(system_id, component_id, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); -} - -/** - * @brief Encode a param_request_read struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param param_request_read C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_param_request_read_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read) -{ - return mavlink_msg_param_request_read_pack_chan(system_id, component_id, chan, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); -} - -/** - * @brief Send a param_request_read message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN]; - _mav_put_int16_t(buf, 0, param_index); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_char_array(buf, 4, param_id, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif -#else - mavlink_param_request_read_t packet; - packet.param_index = param_index; - packet.target_system = target_system; - packet.target_component = target_component; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_param_request_read_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int16_t(buf, 0, param_index); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_char_array(buf, 4, param_id, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif -#else - mavlink_param_request_read_t *packet = (mavlink_param_request_read_t *)msgbuf; - packet->param_index = param_index; - packet->target_system = target_system; - packet->target_component = target_component; - mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE PARAM_REQUEST_READ UNPACKING - - -/** - * @brief Get field target_system from param_request_read message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from param_request_read message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_param_request_read_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field param_id from param_request_read message - * - * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - */ -static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, char *param_id) -{ - return _MAV_RETURN_char_array(msg, param_id, 16, 4); -} - -/** - * @brief Get field param_index from param_request_read message - * - * @return Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) - */ -static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 0); -} - -/** - * @brief Decode a param_request_read message into a struct - * - * @param msg The message to decode - * @param param_request_read C-struct to decode the message contents into - */ -static inline void mavlink_msg_param_request_read_decode(const mavlink_message_t* msg, mavlink_param_request_read_t* param_request_read) -{ -#if MAVLINK_NEED_BYTE_SWAP - param_request_read->param_index = mavlink_msg_param_request_read_get_param_index(msg); - param_request_read->target_system = mavlink_msg_param_request_read_get_target_system(msg); - param_request_read->target_component = mavlink_msg_param_request_read_get_target_component(msg); - mavlink_msg_param_request_read_get_param_id(msg, param_request_read->param_id); -#else - memcpy(param_request_read, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h deleted file mode 100644 index f669af1a2..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h +++ /dev/null @@ -1,297 +0,0 @@ -// MESSAGE PARAM_SET PACKING - -#define MAVLINK_MSG_ID_PARAM_SET 23 - -typedef struct __mavlink_param_set_t -{ - float param_value; ///< Onboard parameter value - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - uint8_t param_type; ///< Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. -} mavlink_param_set_t; - -#define MAVLINK_MSG_ID_PARAM_SET_LEN 23 -#define MAVLINK_MSG_ID_23_LEN 23 - -#define MAVLINK_MSG_ID_PARAM_SET_CRC 168 -#define MAVLINK_MSG_ID_23_CRC 168 - -#define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 16 - -#define MAVLINK_MESSAGE_INFO_PARAM_SET { \ - "PARAM_SET", \ - 5, \ - { { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_set_t, param_value) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_param_set_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_param_set_t, target_component) }, \ - { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 6, offsetof(mavlink_param_set_t, param_id) }, \ - { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_param_set_t, param_type) }, \ - } \ -} - - -/** - * @brief Pack a param_set message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 22, param_type); - _mav_put_char_array(buf, 6, param_id, 16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_SET_LEN); -#else - mavlink_param_set_t packet; - packet.param_value = param_value; - packet.target_system = target_system; - packet.target_component = target_component; - packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PARAM_SET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif -} - -/** - * @brief Pack a param_set message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,const char *param_id,float param_value,uint8_t param_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 22, param_type); - _mav_put_char_array(buf, 6, param_id, 16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_SET_LEN); -#else - mavlink_param_set_t packet; - packet.param_value = param_value; - packet.target_system = target_system; - packet.target_component = target_component; - packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PARAM_SET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif -} - -/** - * @brief Encode a param_set struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param param_set C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_set_t* param_set) -{ - return mavlink_msg_param_set_pack(system_id, component_id, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); -} - -/** - * @brief Encode a param_set struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param param_set C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_param_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_set_t* param_set) -{ - return mavlink_msg_param_set_pack_chan(system_id, component_id, chan, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); -} - -/** - * @brief Send a param_set message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 22, param_type); - _mav_put_char_array(buf, 6, param_id, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif -#else - mavlink_param_set_t packet; - packet.param_value = param_value; - packet.target_system = target_system; - packet.target_component = target_component; - packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)&packet, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)&packet, MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_PARAM_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_param_set_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, param_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 22, param_type); - _mav_put_char_array(buf, 6, param_id, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif -#else - mavlink_param_set_t *packet = (mavlink_param_set_t *)msgbuf; - packet->param_value = param_value; - packet->target_system = target_system; - packet->target_component = target_component; - packet->param_type = param_type; - mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE PARAM_SET UNPACKING - - -/** - * @brief Get field target_system from param_set message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field target_component from param_set message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field param_id from param_set message - * - * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - */ -static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, char *param_id) -{ - return _MAV_RETURN_char_array(msg, param_id, 16, 6); -} - -/** - * @brief Get field param_value from param_set message - * - * @return Onboard parameter value - */ -static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field param_type from param_set message - * - * @return Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. - */ -static inline uint8_t mavlink_msg_param_set_get_param_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 22); -} - -/** - * @brief Decode a param_set message into a struct - * - * @param msg The message to decode - * @param param_set C-struct to decode the message contents into - */ -static inline void mavlink_msg_param_set_decode(const mavlink_message_t* msg, mavlink_param_set_t* param_set) -{ -#if MAVLINK_NEED_BYTE_SWAP - param_set->param_value = mavlink_msg_param_set_get_param_value(msg); - param_set->target_system = mavlink_msg_param_set_get_target_system(msg); - param_set->target_component = mavlink_msg_param_set_get_target_component(msg); - mavlink_msg_param_set_get_param_id(msg, param_set->param_id); - param_set->param_type = mavlink_msg_param_set_get_param_type(msg); -#else - memcpy(param_set, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h deleted file mode 100644 index c27957913..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h +++ /dev/null @@ -1,297 +0,0 @@ -// MESSAGE PARAM_VALUE PACKING - -#define MAVLINK_MSG_ID_PARAM_VALUE 22 - -typedef struct __mavlink_param_value_t -{ - float param_value; ///< Onboard parameter value - uint16_t param_count; ///< Total number of onboard parameters - uint16_t param_index; ///< Index of this onboard parameter - char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - uint8_t param_type; ///< Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. -} mavlink_param_value_t; - -#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 25 -#define MAVLINK_MSG_ID_22_LEN 25 - -#define MAVLINK_MSG_ID_PARAM_VALUE_CRC 220 -#define MAVLINK_MSG_ID_22_CRC 220 - -#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 - -#define MAVLINK_MESSAGE_INFO_PARAM_VALUE { \ - "PARAM_VALUE", \ - 5, \ - { { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_value_t, param_value) }, \ - { "param_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_param_value_t, param_count) }, \ - { "param_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_param_value_t, param_index) }, \ - { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 8, offsetof(mavlink_param_value_t, param_id) }, \ - { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_param_value_t, param_type) }, \ - } \ -} - - -/** - * @brief Pack a param_value message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. - * @param param_count Total number of onboard parameters - * @param param_index Index of this onboard parameter - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint16_t(buf, 4, param_count); - _mav_put_uint16_t(buf, 6, param_index); - _mav_put_uint8_t(buf, 24, param_type); - _mav_put_char_array(buf, 8, param_id, 16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#else - mavlink_param_value_t packet; - packet.param_value = param_value; - packet.param_count = param_count; - packet.param_index = param_index; - packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif -} - -/** - * @brief Pack a param_value message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. - * @param param_count Total number of onboard parameters - * @param param_index Index of this onboard parameter - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const char *param_id,float param_value,uint8_t param_type,uint16_t param_count,uint16_t param_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint16_t(buf, 4, param_count); - _mav_put_uint16_t(buf, 6, param_index); - _mav_put_uint8_t(buf, 24, param_type); - _mav_put_char_array(buf, 8, param_id, 16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#else - mavlink_param_value_t packet; - packet.param_value = param_value; - packet.param_count = param_count; - packet.param_index = param_index; - packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif -} - -/** - * @brief Encode a param_value struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param param_value C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_value_t* param_value) -{ - return mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); -} - -/** - * @brief Encode a param_value struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param param_value C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_param_value_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_value_t* param_value) -{ - return mavlink_msg_param_value_pack_chan(system_id, component_id, chan, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); -} - -/** - * @brief Send a param_value message - * @param chan MAVLink channel to send the message - * - * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. - * @param param_count Total number of onboard parameters - * @param param_index Index of this onboard parameter - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint16_t(buf, 4, param_count); - _mav_put_uint16_t(buf, 6, param_index); - _mav_put_uint8_t(buf, 24, param_type); - _mav_put_char_array(buf, 8, param_id, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif -#else - mavlink_param_value_t packet; - packet.param_value = param_value; - packet.param_count = param_count; - packet.param_index = param_index; - packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_PARAM_VALUE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_param_value_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, param_value); - _mav_put_uint16_t(buf, 4, param_count); - _mav_put_uint16_t(buf, 6, param_index); - _mav_put_uint8_t(buf, 24, param_type); - _mav_put_char_array(buf, 8, param_id, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif -#else - mavlink_param_value_t *packet = (mavlink_param_value_t *)msgbuf; - packet->param_value = param_value; - packet->param_count = param_count; - packet->param_index = param_index; - packet->param_type = param_type; - mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE PARAM_VALUE UNPACKING - - -/** - * @brief Get field param_id from param_value message - * - * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - */ -static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, char *param_id) -{ - return _MAV_RETURN_char_array(msg, param_id, 16, 8); -} - -/** - * @brief Get field param_value from param_value message - * - * @return Onboard parameter value - */ -static inline float mavlink_msg_param_value_get_param_value(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field param_type from param_value message - * - * @return Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. - */ -static inline uint8_t mavlink_msg_param_value_get_param_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 24); -} - -/** - * @brief Get field param_count from param_value message - * - * @return Total number of onboard parameters - */ -static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field param_index from param_value message - * - * @return Index of this onboard parameter - */ -static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Decode a param_value message into a struct - * - * @param msg The message to decode - * @param param_value C-struct to decode the message contents into - */ -static inline void mavlink_msg_param_value_decode(const mavlink_message_t* msg, mavlink_param_value_t* param_value) -{ -#if MAVLINK_NEED_BYTE_SWAP - param_value->param_value = mavlink_msg_param_value_get_param_value(msg); - param_value->param_count = mavlink_msg_param_value_get_param_count(msg); - param_value->param_index = mavlink_msg_param_value_get_param_index(msg); - mavlink_msg_param_value_get_param_id(msg, param_value->param_id); - param_value->param_type = mavlink_msg_param_value_get_param_type(msg); -#else - memcpy(param_value, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h deleted file mode 100644 index b1501ba1f..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE PING PACKING - -#define MAVLINK_MSG_ID_PING 4 - -typedef struct __mavlink_ping_t -{ - uint64_t time_usec; ///< Unix timestamp in microseconds - uint32_t seq; ///< PING sequence - uint8_t target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - uint8_t target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system -} mavlink_ping_t; - -#define MAVLINK_MSG_ID_PING_LEN 14 -#define MAVLINK_MSG_ID_4_LEN 14 - -#define MAVLINK_MSG_ID_PING_CRC 237 -#define MAVLINK_MSG_ID_4_CRC 237 - - - -#define MAVLINK_MESSAGE_INFO_PING { \ - "PING", \ - 4, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_ping_t, time_usec) }, \ - { "seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_ping_t, seq) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_ping_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_ping_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a ping message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Unix timestamp in microseconds - * @param seq PING sequence - * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PING_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_uint32_t(buf, 8, seq); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PING_LEN); -#else - mavlink_ping_t packet; - packet.time_usec = time_usec; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PING_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PING; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_LEN); -#endif -} - -/** - * @brief Pack a ping message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Unix timestamp in microseconds - * @param seq PING sequence - * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,uint32_t seq,uint8_t target_system,uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PING_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_uint32_t(buf, 8, seq); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PING_LEN); -#else - mavlink_ping_t packet; - packet.time_usec = time_usec; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PING_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PING; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PING_LEN); -#endif -} - -/** - * @brief Encode a ping struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param ping C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ping_t* ping) -{ - return mavlink_msg_ping_pack(system_id, component_id, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component); -} - -/** - * @brief Encode a ping struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param ping C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ping_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ping_t* ping) -{ - return mavlink_msg_ping_pack_chan(system_id, component_id, chan, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component); -} - -/** - * @brief Send a ping message - * @param chan MAVLink channel to send the message - * - * @param time_usec Unix timestamp in microseconds - * @param seq PING sequence - * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PING_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_uint32_t(buf, 8, seq); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN); -#endif -#else - mavlink_ping_t packet; - packet.time_usec = time_usec; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)&packet, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)&packet, MAVLINK_MSG_ID_PING_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_PING_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_ping_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_uint32_t(buf, 8, seq); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN); -#endif -#else - mavlink_ping_t *packet = (mavlink_ping_t *)msgbuf; - packet->time_usec = time_usec; - packet->seq = seq; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE PING UNPACKING - - -/** - * @brief Get field time_usec from ping message - * - * @return Unix timestamp in microseconds - */ -static inline uint64_t mavlink_msg_ping_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field seq from ping message - * - * @return PING sequence - */ -static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 8); -} - -/** - * @brief Get field target_system from ping message - * - * @return 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - */ -static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field target_component from ping message - * - * @return 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - */ -static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 13); -} - -/** - * @brief Decode a ping message into a struct - * - * @param msg The message to decode - * @param ping C-struct to decode the message contents into - */ -static inline void mavlink_msg_ping_decode(const mavlink_message_t* msg, mavlink_ping_t* ping) -{ -#if MAVLINK_NEED_BYTE_SWAP - ping->time_usec = mavlink_msg_ping_get_time_usec(msg); - ping->seq = mavlink_msg_ping_get_seq(msg); - ping->target_system = mavlink_msg_ping_get_target_system(msg); - ping->target_component = mavlink_msg_ping_get_target_component(msg); -#else - memcpy(ping, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PING_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h deleted file mode 100644 index 1a6259aa3..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE POWER_STATUS PACKING - -#define MAVLINK_MSG_ID_POWER_STATUS 125 - -typedef struct __mavlink_power_status_t -{ - uint16_t Vcc; ///< 5V rail voltage in millivolts - uint16_t Vservo; ///< servo rail voltage in millivolts - uint16_t flags; ///< power supply status flags (see MAV_POWER_STATUS enum) -} mavlink_power_status_t; - -#define MAVLINK_MSG_ID_POWER_STATUS_LEN 6 -#define MAVLINK_MSG_ID_125_LEN 6 - -#define MAVLINK_MSG_ID_POWER_STATUS_CRC 203 -#define MAVLINK_MSG_ID_125_CRC 203 - - - -#define MAVLINK_MESSAGE_INFO_POWER_STATUS { \ - "POWER_STATUS", \ - 3, \ - { { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_power_status_t, Vcc) }, \ - { "Vservo", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_power_status_t, Vservo) }, \ - { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_power_status_t, flags) }, \ - } \ -} - - -/** - * @brief Pack a power_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param Vcc 5V rail voltage in millivolts - * @param Vservo servo rail voltage in millivolts - * @param flags power supply status flags (see MAV_POWER_STATUS enum) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_power_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t Vcc, uint16_t Vservo, uint16_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; - _mav_put_uint16_t(buf, 0, Vcc); - _mav_put_uint16_t(buf, 2, Vservo); - _mav_put_uint16_t(buf, 4, flags); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); -#else - mavlink_power_status_t packet; - packet.Vcc = Vcc; - packet.Vservo = Vservo; - packet.flags = flags; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POWER_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_LEN); -#endif -} - -/** - * @brief Pack a power_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param Vcc 5V rail voltage in millivolts - * @param Vservo servo rail voltage in millivolts - * @param flags power supply status flags (see MAV_POWER_STATUS enum) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_power_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t Vcc,uint16_t Vservo,uint16_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; - _mav_put_uint16_t(buf, 0, Vcc); - _mav_put_uint16_t(buf, 2, Vservo); - _mav_put_uint16_t(buf, 4, flags); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); -#else - mavlink_power_status_t packet; - packet.Vcc = Vcc; - packet.Vservo = Vservo; - packet.flags = flags; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POWER_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POWER_STATUS_LEN); -#endif -} - -/** - * @brief Encode a power_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param power_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_power_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_power_status_t* power_status) -{ - return mavlink_msg_power_status_pack(system_id, component_id, msg, power_status->Vcc, power_status->Vservo, power_status->flags); -} - -/** - * @brief Encode a power_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param power_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_power_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_power_status_t* power_status) -{ - return mavlink_msg_power_status_pack_chan(system_id, component_id, chan, msg, power_status->Vcc, power_status->Vservo, power_status->flags); -} - -/** - * @brief Send a power_status message - * @param chan MAVLink channel to send the message - * - * @param Vcc 5V rail voltage in millivolts - * @param Vservo servo rail voltage in millivolts - * @param flags power supply status flags (see MAV_POWER_STATUS enum) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_power_status_send(mavlink_channel_t chan, uint16_t Vcc, uint16_t Vservo, uint16_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; - _mav_put_uint16_t(buf, 0, Vcc); - _mav_put_uint16_t(buf, 2, Vservo); - _mav_put_uint16_t(buf, 4, flags); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); -#endif -#else - mavlink_power_status_t packet; - packet.Vcc = Vcc; - packet.Vservo = Vservo; - packet.flags = flags; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_POWER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_power_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Vcc, uint16_t Vservo, uint16_t flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, Vcc); - _mav_put_uint16_t(buf, 2, Vservo); - _mav_put_uint16_t(buf, 4, flags); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); -#endif -#else - mavlink_power_status_t *packet = (mavlink_power_status_t *)msgbuf; - packet->Vcc = Vcc; - packet->Vservo = Vservo; - packet->flags = flags; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)packet, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE POWER_STATUS UNPACKING - - -/** - * @brief Get field Vcc from power_status message - * - * @return 5V rail voltage in millivolts - */ -static inline uint16_t mavlink_msg_power_status_get_Vcc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field Vservo from power_status message - * - * @return servo rail voltage in millivolts - */ -static inline uint16_t mavlink_msg_power_status_get_Vservo(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field flags from power_status message - * - * @return power supply status flags (see MAV_POWER_STATUS enum) - */ -static inline uint16_t mavlink_msg_power_status_get_flags(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Decode a power_status message into a struct - * - * @param msg The message to decode - * @param power_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_power_status_decode(const mavlink_message_t* msg, mavlink_power_status_t* power_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - power_status->Vcc = mavlink_msg_power_status_get_Vcc(msg); - power_status->Vservo = mavlink_msg_power_status_get_Vservo(msg); - power_status->flags = mavlink_msg_power_status_get_flags(msg); -#else - memcpy(power_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POWER_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h deleted file mode 100644 index 5763008fe..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE RADIO_STATUS PACKING - -#define MAVLINK_MSG_ID_RADIO_STATUS 109 - -typedef struct __mavlink_radio_status_t -{ - uint16_t rxerrors; ///< receive errors - uint16_t fixed; ///< count of error corrected packets - uint8_t rssi; ///< local signal strength - uint8_t remrssi; ///< remote signal strength - uint8_t txbuf; ///< how full the tx buffer is as a percentage - uint8_t noise; ///< background noise level - uint8_t remnoise; ///< remote background noise level -} mavlink_radio_status_t; - -#define MAVLINK_MSG_ID_RADIO_STATUS_LEN 9 -#define MAVLINK_MSG_ID_109_LEN 9 - -#define MAVLINK_MSG_ID_RADIO_STATUS_CRC 185 -#define MAVLINK_MSG_ID_109_CRC 185 - - - -#define MAVLINK_MESSAGE_INFO_RADIO_STATUS { \ - "RADIO_STATUS", \ - 7, \ - { { "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_radio_status_t, rxerrors) }, \ - { "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_radio_status_t, fixed) }, \ - { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_radio_status_t, rssi) }, \ - { "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_radio_status_t, remrssi) }, \ - { "txbuf", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_radio_status_t, txbuf) }, \ - { "noise", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_radio_status_t, noise) }, \ - { "remnoise", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_radio_status_t, remnoise) }, \ - } \ -} - - -/** - * @brief Pack a radio_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param rssi local signal strength - * @param remrssi remote signal strength - * @param txbuf how full the tx buffer is as a percentage - * @param noise background noise level - * @param remnoise remote background noise level - * @param rxerrors receive errors - * @param fixed count of error corrected packets - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_radio_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RADIO_STATUS_LEN]; - _mav_put_uint16_t(buf, 0, rxerrors); - _mav_put_uint16_t(buf, 2, fixed); - _mav_put_uint8_t(buf, 4, rssi); - _mav_put_uint8_t(buf, 5, remrssi); - _mav_put_uint8_t(buf, 6, txbuf); - _mav_put_uint8_t(buf, 7, noise); - _mav_put_uint8_t(buf, 8, remnoise); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); -#else - mavlink_radio_status_t packet; - packet.rxerrors = rxerrors; - packet.fixed = fixed; - packet.rssi = rssi; - packet.remrssi = remrssi; - packet.txbuf = txbuf; - packet.noise = noise; - packet.remnoise = remnoise; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RADIO_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_STATUS_LEN); -#endif -} - -/** - * @brief Pack a radio_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rssi local signal strength - * @param remrssi remote signal strength - * @param txbuf how full the tx buffer is as a percentage - * @param noise background noise level - * @param remnoise remote background noise level - * @param rxerrors receive errors - * @param fixed count of error corrected packets - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_radio_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t rssi,uint8_t remrssi,uint8_t txbuf,uint8_t noise,uint8_t remnoise,uint16_t rxerrors,uint16_t fixed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RADIO_STATUS_LEN]; - _mav_put_uint16_t(buf, 0, rxerrors); - _mav_put_uint16_t(buf, 2, fixed); - _mav_put_uint8_t(buf, 4, rssi); - _mav_put_uint8_t(buf, 5, remrssi); - _mav_put_uint8_t(buf, 6, txbuf); - _mav_put_uint8_t(buf, 7, noise); - _mav_put_uint8_t(buf, 8, remnoise); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); -#else - mavlink_radio_status_t packet; - packet.rxerrors = rxerrors; - packet.fixed = fixed; - packet.rssi = rssi; - packet.remrssi = remrssi; - packet.txbuf = txbuf; - packet.noise = noise; - packet.remnoise = remnoise; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RADIO_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_STATUS_LEN); -#endif -} - -/** - * @brief Encode a radio_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param radio_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_radio_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_status_t* radio_status) -{ - return mavlink_msg_radio_status_pack(system_id, component_id, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed); -} - -/** - * @brief Encode a radio_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param radio_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_radio_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_status_t* radio_status) -{ - return mavlink_msg_radio_status_pack_chan(system_id, component_id, chan, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed); -} - -/** - * @brief Send a radio_status message - * @param chan MAVLink channel to send the message - * - * @param rssi local signal strength - * @param remrssi remote signal strength - * @param txbuf how full the tx buffer is as a percentage - * @param noise background noise level - * @param remnoise remote background noise level - * @param rxerrors receive errors - * @param fixed count of error corrected packets - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_radio_status_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RADIO_STATUS_LEN]; - _mav_put_uint16_t(buf, 0, rxerrors); - _mav_put_uint16_t(buf, 2, fixed); - _mav_put_uint8_t(buf, 4, rssi); - _mav_put_uint8_t(buf, 5, remrssi); - _mav_put_uint8_t(buf, 6, txbuf); - _mav_put_uint8_t(buf, 7, noise); - _mav_put_uint8_t(buf, 8, remnoise); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); -#endif -#else - mavlink_radio_status_t packet; - packet.rxerrors = rxerrors; - packet.fixed = fixed; - packet.rssi = rssi; - packet.remrssi = remrssi; - packet.txbuf = txbuf; - packet.noise = noise; - packet.remnoise = remnoise; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)&packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)&packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_RADIO_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_radio_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, rxerrors); - _mav_put_uint16_t(buf, 2, fixed); - _mav_put_uint8_t(buf, 4, rssi); - _mav_put_uint8_t(buf, 5, remrssi); - _mav_put_uint8_t(buf, 6, txbuf); - _mav_put_uint8_t(buf, 7, noise); - _mav_put_uint8_t(buf, 8, remnoise); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); -#endif -#else - mavlink_radio_status_t *packet = (mavlink_radio_status_t *)msgbuf; - packet->rxerrors = rxerrors; - packet->fixed = fixed; - packet->rssi = rssi; - packet->remrssi = remrssi; - packet->txbuf = txbuf; - packet->noise = noise; - packet->remnoise = remnoise; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE RADIO_STATUS UNPACKING - - -/** - * @brief Get field rssi from radio_status message - * - * @return local signal strength - */ -static inline uint8_t mavlink_msg_radio_status_get_rssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field remrssi from radio_status message - * - * @return remote signal strength - */ -static inline uint8_t mavlink_msg_radio_status_get_remrssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field txbuf from radio_status message - * - * @return how full the tx buffer is as a percentage - */ -static inline uint8_t mavlink_msg_radio_status_get_txbuf(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field noise from radio_status message - * - * @return background noise level - */ -static inline uint8_t mavlink_msg_radio_status_get_noise(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field remnoise from radio_status message - * - * @return remote background noise level - */ -static inline uint8_t mavlink_msg_radio_status_get_remnoise(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field rxerrors from radio_status message - * - * @return receive errors - */ -static inline uint16_t mavlink_msg_radio_status_get_rxerrors(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field fixed from radio_status message - * - * @return count of error corrected packets - */ -static inline uint16_t mavlink_msg_radio_status_get_fixed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Decode a radio_status message into a struct - * - * @param msg The message to decode - * @param radio_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_radio_status_decode(const mavlink_message_t* msg, mavlink_radio_status_t* radio_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - radio_status->rxerrors = mavlink_msg_radio_status_get_rxerrors(msg); - radio_status->fixed = mavlink_msg_radio_status_get_fixed(msg); - radio_status->rssi = mavlink_msg_radio_status_get_rssi(msg); - radio_status->remrssi = mavlink_msg_radio_status_get_remrssi(msg); - radio_status->txbuf = mavlink_msg_radio_status_get_txbuf(msg); - radio_status->noise = mavlink_msg_radio_status_get_noise(msg); - radio_status->remnoise = mavlink_msg_radio_status_get_remnoise(msg); -#else - memcpy(radio_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RADIO_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h deleted file mode 100644 index a98e8ce72..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h +++ /dev/null @@ -1,425 +0,0 @@ -// MESSAGE RAW_IMU PACKING - -#define MAVLINK_MSG_ID_RAW_IMU 27 - -typedef struct __mavlink_raw_imu_t -{ - uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int16_t xacc; ///< X acceleration (raw) - int16_t yacc; ///< Y acceleration (raw) - int16_t zacc; ///< Z acceleration (raw) - int16_t xgyro; ///< Angular speed around X axis (raw) - int16_t ygyro; ///< Angular speed around Y axis (raw) - int16_t zgyro; ///< Angular speed around Z axis (raw) - int16_t xmag; ///< X Magnetic field (raw) - int16_t ymag; ///< Y Magnetic field (raw) - int16_t zmag; ///< Z Magnetic field (raw) -} mavlink_raw_imu_t; - -#define MAVLINK_MSG_ID_RAW_IMU_LEN 26 -#define MAVLINK_MSG_ID_27_LEN 26 - -#define MAVLINK_MSG_ID_RAW_IMU_CRC 144 -#define MAVLINK_MSG_ID_27_CRC 144 - - - -#define MAVLINK_MESSAGE_INFO_RAW_IMU { \ - "RAW_IMU", \ - 10, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_imu_t, time_usec) }, \ - { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_imu_t, xacc) }, \ - { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_imu_t, yacc) }, \ - { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_raw_imu_t, zacc) }, \ - { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_imu_t, xgyro) }, \ - { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_raw_imu_t, ygyro) }, \ - { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_raw_imu_t, zgyro) }, \ - { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_raw_imu_t, xmag) }, \ - { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_raw_imu_t, ymag) }, \ - { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_raw_imu_t, zmag) }, \ - } \ -} - - -/** - * @brief Pack a raw_imu message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param xacc X acceleration (raw) - * @param yacc Y acceleration (raw) - * @param zacc Z acceleration (raw) - * @param xgyro Angular speed around X axis (raw) - * @param ygyro Angular speed around Y axis (raw) - * @param zgyro Angular speed around Z axis (raw) - * @param xmag X Magnetic field (raw) - * @param ymag Y Magnetic field (raw) - * @param zmag Z Magnetic field (raw) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int16_t(buf, 8, xacc); - _mav_put_int16_t(buf, 10, yacc); - _mav_put_int16_t(buf, 12, zacc); - _mav_put_int16_t(buf, 14, xgyro); - _mav_put_int16_t(buf, 16, ygyro); - _mav_put_int16_t(buf, 18, zgyro); - _mav_put_int16_t(buf, 20, xmag); - _mav_put_int16_t(buf, 22, ymag); - _mav_put_int16_t(buf, 24, zmag); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN); -#else - mavlink_raw_imu_t packet; - packet.time_usec = time_usec; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RAW_IMU; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_IMU_LEN); -#endif -} - -/** - * @brief Pack a raw_imu message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param xacc X acceleration (raw) - * @param yacc Y acceleration (raw) - * @param zacc Z acceleration (raw) - * @param xgyro Angular speed around X axis (raw) - * @param ygyro Angular speed around Y axis (raw) - * @param zgyro Angular speed around Z axis (raw) - * @param xmag X Magnetic field (raw) - * @param ymag Y Magnetic field (raw) - * @param zmag Z Magnetic field (raw) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int16_t(buf, 8, xacc); - _mav_put_int16_t(buf, 10, yacc); - _mav_put_int16_t(buf, 12, zacc); - _mav_put_int16_t(buf, 14, xgyro); - _mav_put_int16_t(buf, 16, ygyro); - _mav_put_int16_t(buf, 18, zgyro); - _mav_put_int16_t(buf, 20, xmag); - _mav_put_int16_t(buf, 22, ymag); - _mav_put_int16_t(buf, 24, zmag); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN); -#else - mavlink_raw_imu_t packet; - packet.time_usec = time_usec; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RAW_IMU; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_IMU_LEN); -#endif -} - -/** - * @brief Encode a raw_imu struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param raw_imu C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) -{ - return mavlink_msg_raw_imu_pack(system_id, component_id, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); -} - -/** - * @brief Encode a raw_imu struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param raw_imu C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_raw_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) -{ - return mavlink_msg_raw_imu_pack_chan(system_id, component_id, chan, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); -} - -/** - * @brief Send a raw_imu message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param xacc X acceleration (raw) - * @param yacc Y acceleration (raw) - * @param zacc Z acceleration (raw) - * @param xgyro Angular speed around X axis (raw) - * @param ygyro Angular speed around Y axis (raw) - * @param zgyro Angular speed around Z axis (raw) - * @param xmag X Magnetic field (raw) - * @param ymag Y Magnetic field (raw) - * @param zmag Z Magnetic field (raw) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int16_t(buf, 8, xacc); - _mav_put_int16_t(buf, 10, yacc); - _mav_put_int16_t(buf, 12, zacc); - _mav_put_int16_t(buf, 14, xgyro); - _mav_put_int16_t(buf, 16, ygyro); - _mav_put_int16_t(buf, 18, zgyro); - _mav_put_int16_t(buf, 20, xmag); - _mav_put_int16_t(buf, 22, ymag); - _mav_put_int16_t(buf, 24, zmag); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN); -#endif -#else - mavlink_raw_imu_t packet; - packet.time_usec = time_usec; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)&packet, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)&packet, MAVLINK_MSG_ID_RAW_IMU_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_RAW_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int16_t(buf, 8, xacc); - _mav_put_int16_t(buf, 10, yacc); - _mav_put_int16_t(buf, 12, zacc); - _mav_put_int16_t(buf, 14, xgyro); - _mav_put_int16_t(buf, 16, ygyro); - _mav_put_int16_t(buf, 18, zgyro); - _mav_put_int16_t(buf, 20, xmag); - _mav_put_int16_t(buf, 22, ymag); - _mav_put_int16_t(buf, 24, zmag); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN); -#endif -#else - mavlink_raw_imu_t *packet = (mavlink_raw_imu_t *)msgbuf; - packet->time_usec = time_usec; - packet->xacc = xacc; - packet->yacc = yacc; - packet->zacc = zacc; - packet->xgyro = xgyro; - packet->ygyro = ygyro; - packet->zgyro = zgyro; - packet->xmag = xmag; - packet->ymag = ymag; - packet->zmag = zmag; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE RAW_IMU UNPACKING - - -/** - * @brief Get field time_usec from raw_imu message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_raw_imu_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field xacc from raw_imu message - * - * @return X acceleration (raw) - */ -static inline int16_t mavlink_msg_raw_imu_get_xacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field yacc from raw_imu message - * - * @return Y acceleration (raw) - */ -static inline int16_t mavlink_msg_raw_imu_get_yacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 10); -} - -/** - * @brief Get field zacc from raw_imu message - * - * @return Z acceleration (raw) - */ -static inline int16_t mavlink_msg_raw_imu_get_zacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Get field xgyro from raw_imu message - * - * @return Angular speed around X axis (raw) - */ -static inline int16_t mavlink_msg_raw_imu_get_xgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 14); -} - -/** - * @brief Get field ygyro from raw_imu message - * - * @return Angular speed around Y axis (raw) - */ -static inline int16_t mavlink_msg_raw_imu_get_ygyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 16); -} - -/** - * @brief Get field zgyro from raw_imu message - * - * @return Angular speed around Z axis (raw) - */ -static inline int16_t mavlink_msg_raw_imu_get_zgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 18); -} - -/** - * @brief Get field xmag from raw_imu message - * - * @return X Magnetic field (raw) - */ -static inline int16_t mavlink_msg_raw_imu_get_xmag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Get field ymag from raw_imu message - * - * @return Y Magnetic field (raw) - */ -static inline int16_t mavlink_msg_raw_imu_get_ymag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 22); -} - -/** - * @brief Get field zmag from raw_imu message - * - * @return Z Magnetic field (raw) - */ -static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 24); -} - -/** - * @brief Decode a raw_imu message into a struct - * - * @param msg The message to decode - * @param raw_imu C-struct to decode the message contents into - */ -static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavlink_raw_imu_t* raw_imu) -{ -#if MAVLINK_NEED_BYTE_SWAP - raw_imu->time_usec = mavlink_msg_raw_imu_get_time_usec(msg); - raw_imu->xacc = mavlink_msg_raw_imu_get_xacc(msg); - raw_imu->yacc = mavlink_msg_raw_imu_get_yacc(msg); - raw_imu->zacc = mavlink_msg_raw_imu_get_zacc(msg); - raw_imu->xgyro = mavlink_msg_raw_imu_get_xgyro(msg); - raw_imu->ygyro = mavlink_msg_raw_imu_get_ygyro(msg); - raw_imu->zgyro = mavlink_msg_raw_imu_get_zgyro(msg); - raw_imu->xmag = mavlink_msg_raw_imu_get_xmag(msg); - raw_imu->ymag = mavlink_msg_raw_imu_get_ymag(msg); - raw_imu->zmag = mavlink_msg_raw_imu_get_zmag(msg); -#else - memcpy(raw_imu, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RAW_IMU_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h deleted file mode 100644 index 6bd37fcae..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE RAW_PRESSURE PACKING - -#define MAVLINK_MSG_ID_RAW_PRESSURE 28 - -typedef struct __mavlink_raw_pressure_t -{ - uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int16_t press_abs; ///< Absolute pressure (raw) - int16_t press_diff1; ///< Differential pressure 1 (raw) - int16_t press_diff2; ///< Differential pressure 2 (raw) - int16_t temperature; ///< Raw Temperature measurement (raw) -} mavlink_raw_pressure_t; - -#define MAVLINK_MSG_ID_RAW_PRESSURE_LEN 16 -#define MAVLINK_MSG_ID_28_LEN 16 - -#define MAVLINK_MSG_ID_RAW_PRESSURE_CRC 67 -#define MAVLINK_MSG_ID_28_CRC 67 - - - -#define MAVLINK_MESSAGE_INFO_RAW_PRESSURE { \ - "RAW_PRESSURE", \ - 5, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_pressure_t, time_usec) }, \ - { "press_abs", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_pressure_t, press_abs) }, \ - { "press_diff1", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_pressure_t, press_diff1) }, \ - { "press_diff2", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_raw_pressure_t, press_diff2) }, \ - { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_pressure_t, temperature) }, \ - } \ -} - - -/** - * @brief Pack a raw_pressure message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param press_abs Absolute pressure (raw) - * @param press_diff1 Differential pressure 1 (raw) - * @param press_diff2 Differential pressure 2 (raw) - * @param temperature Raw Temperature measurement (raw) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RAW_PRESSURE_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int16_t(buf, 8, press_abs); - _mav_put_int16_t(buf, 10, press_diff1); - _mav_put_int16_t(buf, 12, press_diff2); - _mav_put_int16_t(buf, 14, temperature); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); -#else - mavlink_raw_pressure_t packet; - packet.time_usec = time_usec; - packet.press_abs = press_abs; - packet.press_diff1 = press_diff1; - packet.press_diff2 = press_diff2; - packet.temperature = temperature; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); -#endif -} - -/** - * @brief Pack a raw_pressure message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param press_abs Absolute pressure (raw) - * @param press_diff1 Differential pressure 1 (raw) - * @param press_diff2 Differential pressure 2 (raw) - * @param temperature Raw Temperature measurement (raw) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_usec,int16_t press_abs,int16_t press_diff1,int16_t press_diff2,int16_t temperature) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RAW_PRESSURE_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int16_t(buf, 8, press_abs); - _mav_put_int16_t(buf, 10, press_diff1); - _mav_put_int16_t(buf, 12, press_diff2); - _mav_put_int16_t(buf, 14, temperature); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); -#else - mavlink_raw_pressure_t packet; - packet.time_usec = time_usec; - packet.press_abs = press_abs; - packet.press_diff1 = press_diff1; - packet.press_diff2 = press_diff2; - packet.temperature = temperature; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); -#endif -} - -/** - * @brief Encode a raw_pressure struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param raw_pressure C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_pressure_t* raw_pressure) -{ - return mavlink_msg_raw_pressure_pack(system_id, component_id, msg, raw_pressure->time_usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); -} - -/** - * @brief Encode a raw_pressure struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param raw_pressure C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_raw_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_pressure_t* raw_pressure) -{ - return mavlink_msg_raw_pressure_pack_chan(system_id, component_id, chan, msg, raw_pressure->time_usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); -} - -/** - * @brief Send a raw_pressure message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param press_abs Absolute pressure (raw) - * @param press_diff1 Differential pressure 1 (raw) - * @param press_diff2 Differential pressure 2 (raw) - * @param temperature Raw Temperature measurement (raw) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_t time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RAW_PRESSURE_LEN]; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int16_t(buf, 8, press_abs); - _mav_put_int16_t(buf, 10, press_diff1); - _mav_put_int16_t(buf, 12, press_diff2); - _mav_put_int16_t(buf, 14, temperature); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); -#endif -#else - mavlink_raw_pressure_t packet; - packet.time_usec = time_usec; - packet.press_abs = press_abs; - packet.press_diff1 = press_diff1; - packet.press_diff2 = press_diff2; - packet.temperature = temperature; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_RAW_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_raw_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_usec); - _mav_put_int16_t(buf, 8, press_abs); - _mav_put_int16_t(buf, 10, press_diff1); - _mav_put_int16_t(buf, 12, press_diff2); - _mav_put_int16_t(buf, 14, temperature); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); -#endif -#else - mavlink_raw_pressure_t *packet = (mavlink_raw_pressure_t *)msgbuf; - packet->time_usec = time_usec; - packet->press_abs = press_abs; - packet->press_diff1 = press_diff1; - packet->press_diff2 = press_diff2; - packet->temperature = temperature; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE RAW_PRESSURE UNPACKING - - -/** - * @brief Get field time_usec from raw_pressure message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_raw_pressure_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field press_abs from raw_pressure message - * - * @return Absolute pressure (raw) - */ -static inline int16_t mavlink_msg_raw_pressure_get_press_abs(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field press_diff1 from raw_pressure message - * - * @return Differential pressure 1 (raw) - */ -static inline int16_t mavlink_msg_raw_pressure_get_press_diff1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 10); -} - -/** - * @brief Get field press_diff2 from raw_pressure message - * - * @return Differential pressure 2 (raw) - */ -static inline int16_t mavlink_msg_raw_pressure_get_press_diff2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Get field temperature from raw_pressure message - * - * @return Raw Temperature measurement (raw) - */ -static inline int16_t mavlink_msg_raw_pressure_get_temperature(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 14); -} - -/** - * @brief Decode a raw_pressure message into a struct - * - * @param msg The message to decode - * @param raw_pressure C-struct to decode the message contents into - */ -static inline void mavlink_msg_raw_pressure_decode(const mavlink_message_t* msg, mavlink_raw_pressure_t* raw_pressure) -{ -#if MAVLINK_NEED_BYTE_SWAP - raw_pressure->time_usec = mavlink_msg_raw_pressure_get_time_usec(msg); - raw_pressure->press_abs = mavlink_msg_raw_pressure_get_press_abs(msg); - raw_pressure->press_diff1 = mavlink_msg_raw_pressure_get_press_diff1(msg); - raw_pressure->press_diff2 = mavlink_msg_raw_pressure_get_press_diff2(msg); - raw_pressure->temperature = mavlink_msg_raw_pressure_get_temperature(msg); -#else - memcpy(raw_pressure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RAW_PRESSURE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h deleted file mode 100644 index 479af122f..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h +++ /dev/null @@ -1,689 +0,0 @@ -// MESSAGE RC_CHANNELS PACKING - -#define MAVLINK_MSG_ID_RC_CHANNELS 65 - -typedef struct __mavlink_rc_channels_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan9_raw; ///< RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan10_raw; ///< RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan11_raw; ///< RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan12_raw; ///< RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan13_raw; ///< RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan14_raw; ///< RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan15_raw; ///< RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan16_raw; ///< RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan17_raw; ///< RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan18_raw; ///< RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint8_t chancount; ///< Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. - uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. -} mavlink_rc_channels_t; - -#define MAVLINK_MSG_ID_RC_CHANNELS_LEN 42 -#define MAVLINK_MSG_ID_65_LEN 42 - -#define MAVLINK_MSG_ID_RC_CHANNELS_CRC 118 -#define MAVLINK_MSG_ID_65_CRC 118 - - - -#define MAVLINK_MESSAGE_INFO_RC_CHANNELS { \ - "RC_CHANNELS", \ - 21, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_t, time_boot_ms) }, \ - { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_t, chan1_raw) }, \ - { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_t, chan2_raw) }, \ - { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_t, chan3_raw) }, \ - { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_t, chan4_raw) }, \ - { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_t, chan5_raw) }, \ - { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_t, chan6_raw) }, \ - { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_rc_channels_t, chan7_raw) }, \ - { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_t, chan8_raw) }, \ - { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_rc_channels_t, chan9_raw) }, \ - { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_rc_channels_t, chan10_raw) }, \ - { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_rc_channels_t, chan11_raw) }, \ - { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_rc_channels_t, chan12_raw) }, \ - { "chan13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_rc_channels_t, chan13_raw) }, \ - { "chan14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_rc_channels_t, chan14_raw) }, \ - { "chan15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_rc_channels_t, chan15_raw) }, \ - { "chan16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_rc_channels_t, chan16_raw) }, \ - { "chan17_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_rc_channels_t, chan17_raw) }, \ - { "chan18_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 38, offsetof(mavlink_rc_channels_t, chan18_raw) }, \ - { "chancount", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_rc_channels_t, chancount) }, \ - { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_rc_channels_t, rssi) }, \ - } \ -} - - -/** - * @brief Pack a rc_channels message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. - * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rc_channels_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, chan1_raw); - _mav_put_uint16_t(buf, 6, chan2_raw); - _mav_put_uint16_t(buf, 8, chan3_raw); - _mav_put_uint16_t(buf, 10, chan4_raw); - _mav_put_uint16_t(buf, 12, chan5_raw); - _mav_put_uint16_t(buf, 14, chan6_raw); - _mav_put_uint16_t(buf, 16, chan7_raw); - _mav_put_uint16_t(buf, 18, chan8_raw); - _mav_put_uint16_t(buf, 20, chan9_raw); - _mav_put_uint16_t(buf, 22, chan10_raw); - _mav_put_uint16_t(buf, 24, chan11_raw); - _mav_put_uint16_t(buf, 26, chan12_raw); - _mav_put_uint16_t(buf, 28, chan13_raw); - _mav_put_uint16_t(buf, 30, chan14_raw); - _mav_put_uint16_t(buf, 32, chan15_raw); - _mav_put_uint16_t(buf, 34, chan16_raw); - _mav_put_uint16_t(buf, 36, chan17_raw); - _mav_put_uint16_t(buf, 38, chan18_raw); - _mav_put_uint8_t(buf, 40, chancount); - _mav_put_uint8_t(buf, 41, rssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#else - mavlink_rc_channels_t packet; - packet.time_boot_ms = time_boot_ms; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - packet.chan9_raw = chan9_raw; - packet.chan10_raw = chan10_raw; - packet.chan11_raw = chan11_raw; - packet.chan12_raw = chan12_raw; - packet.chan13_raw = chan13_raw; - packet.chan14_raw = chan14_raw; - packet.chan15_raw = chan15_raw; - packet.chan16_raw = chan16_raw; - packet.chan17_raw = chan17_raw; - packet.chan18_raw = chan18_raw; - packet.chancount = chancount; - packet.rssi = rssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif -} - -/** - * @brief Pack a rc_channels message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. - * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rc_channels_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t chancount,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint16_t chan9_raw,uint16_t chan10_raw,uint16_t chan11_raw,uint16_t chan12_raw,uint16_t chan13_raw,uint16_t chan14_raw,uint16_t chan15_raw,uint16_t chan16_raw,uint16_t chan17_raw,uint16_t chan18_raw,uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, chan1_raw); - _mav_put_uint16_t(buf, 6, chan2_raw); - _mav_put_uint16_t(buf, 8, chan3_raw); - _mav_put_uint16_t(buf, 10, chan4_raw); - _mav_put_uint16_t(buf, 12, chan5_raw); - _mav_put_uint16_t(buf, 14, chan6_raw); - _mav_put_uint16_t(buf, 16, chan7_raw); - _mav_put_uint16_t(buf, 18, chan8_raw); - _mav_put_uint16_t(buf, 20, chan9_raw); - _mav_put_uint16_t(buf, 22, chan10_raw); - _mav_put_uint16_t(buf, 24, chan11_raw); - _mav_put_uint16_t(buf, 26, chan12_raw); - _mav_put_uint16_t(buf, 28, chan13_raw); - _mav_put_uint16_t(buf, 30, chan14_raw); - _mav_put_uint16_t(buf, 32, chan15_raw); - _mav_put_uint16_t(buf, 34, chan16_raw); - _mav_put_uint16_t(buf, 36, chan17_raw); - _mav_put_uint16_t(buf, 38, chan18_raw); - _mav_put_uint8_t(buf, 40, chancount); - _mav_put_uint8_t(buf, 41, rssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#else - mavlink_rc_channels_t packet; - packet.time_boot_ms = time_boot_ms; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - packet.chan9_raw = chan9_raw; - packet.chan10_raw = chan10_raw; - packet.chan11_raw = chan11_raw; - packet.chan12_raw = chan12_raw; - packet.chan13_raw = chan13_raw; - packet.chan14_raw = chan14_raw; - packet.chan15_raw = chan15_raw; - packet.chan16_raw = chan16_raw; - packet.chan17_raw = chan17_raw; - packet.chan18_raw = chan18_raw; - packet.chancount = chancount; - packet.rssi = rssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif -} - -/** - * @brief Encode a rc_channels struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param rc_channels C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rc_channels_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels) -{ - return mavlink_msg_rc_channels_pack(system_id, component_id, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); -} - -/** - * @brief Encode a rc_channels struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rc_channels C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rc_channels_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels) -{ - return mavlink_msg_rc_channels_pack_chan(system_id, component_id, chan, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); -} - -/** - * @brief Send a rc_channels message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. - * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_rc_channels_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, chan1_raw); - _mav_put_uint16_t(buf, 6, chan2_raw); - _mav_put_uint16_t(buf, 8, chan3_raw); - _mav_put_uint16_t(buf, 10, chan4_raw); - _mav_put_uint16_t(buf, 12, chan5_raw); - _mav_put_uint16_t(buf, 14, chan6_raw); - _mav_put_uint16_t(buf, 16, chan7_raw); - _mav_put_uint16_t(buf, 18, chan8_raw); - _mav_put_uint16_t(buf, 20, chan9_raw); - _mav_put_uint16_t(buf, 22, chan10_raw); - _mav_put_uint16_t(buf, 24, chan11_raw); - _mav_put_uint16_t(buf, 26, chan12_raw); - _mav_put_uint16_t(buf, 28, chan13_raw); - _mav_put_uint16_t(buf, 30, chan14_raw); - _mav_put_uint16_t(buf, 32, chan15_raw); - _mav_put_uint16_t(buf, 34, chan16_raw); - _mav_put_uint16_t(buf, 36, chan17_raw); - _mav_put_uint16_t(buf, 38, chan18_raw); - _mav_put_uint8_t(buf, 40, chancount); - _mav_put_uint8_t(buf, 41, rssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif -#else - mavlink_rc_channels_t packet; - packet.time_boot_ms = time_boot_ms; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - packet.chan9_raw = chan9_raw; - packet.chan10_raw = chan10_raw; - packet.chan11_raw = chan11_raw; - packet.chan12_raw = chan12_raw; - packet.chan13_raw = chan13_raw; - packet.chan14_raw = chan14_raw; - packet.chan15_raw = chan15_raw; - packet.chan16_raw = chan16_raw; - packet.chan17_raw = chan17_raw; - packet.chan18_raw = chan18_raw; - packet.chancount = chancount; - packet.rssi = rssi; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_RC_CHANNELS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_rc_channels_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, chan1_raw); - _mav_put_uint16_t(buf, 6, chan2_raw); - _mav_put_uint16_t(buf, 8, chan3_raw); - _mav_put_uint16_t(buf, 10, chan4_raw); - _mav_put_uint16_t(buf, 12, chan5_raw); - _mav_put_uint16_t(buf, 14, chan6_raw); - _mav_put_uint16_t(buf, 16, chan7_raw); - _mav_put_uint16_t(buf, 18, chan8_raw); - _mav_put_uint16_t(buf, 20, chan9_raw); - _mav_put_uint16_t(buf, 22, chan10_raw); - _mav_put_uint16_t(buf, 24, chan11_raw); - _mav_put_uint16_t(buf, 26, chan12_raw); - _mav_put_uint16_t(buf, 28, chan13_raw); - _mav_put_uint16_t(buf, 30, chan14_raw); - _mav_put_uint16_t(buf, 32, chan15_raw); - _mav_put_uint16_t(buf, 34, chan16_raw); - _mav_put_uint16_t(buf, 36, chan17_raw); - _mav_put_uint16_t(buf, 38, chan18_raw); - _mav_put_uint8_t(buf, 40, chancount); - _mav_put_uint8_t(buf, 41, rssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif -#else - mavlink_rc_channels_t *packet = (mavlink_rc_channels_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->chan1_raw = chan1_raw; - packet->chan2_raw = chan2_raw; - packet->chan3_raw = chan3_raw; - packet->chan4_raw = chan4_raw; - packet->chan5_raw = chan5_raw; - packet->chan6_raw = chan6_raw; - packet->chan7_raw = chan7_raw; - packet->chan8_raw = chan8_raw; - packet->chan9_raw = chan9_raw; - packet->chan10_raw = chan10_raw; - packet->chan11_raw = chan11_raw; - packet->chan12_raw = chan12_raw; - packet->chan13_raw = chan13_raw; - packet->chan14_raw = chan14_raw; - packet->chan15_raw = chan15_raw; - packet->chan16_raw = chan16_raw; - packet->chan17_raw = chan17_raw; - packet->chan18_raw = chan18_raw; - packet->chancount = chancount; - packet->rssi = rssi; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE RC_CHANNELS UNPACKING - - -/** - * @brief Get field time_boot_ms from rc_channels message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_rc_channels_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field chancount from rc_channels message - * - * @return Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. - */ -static inline uint8_t mavlink_msg_rc_channels_get_chancount(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 40); -} - -/** - * @brief Get field chan1_raw from rc_channels message - * - * @return RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan1_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field chan2_raw from rc_channels message - * - * @return RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan2_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field chan3_raw from rc_channels message - * - * @return RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan3_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field chan4_raw from rc_channels message - * - * @return RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan4_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Get field chan5_raw from rc_channels message - * - * @return RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan5_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field chan6_raw from rc_channels message - * - * @return RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan6_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 14); -} - -/** - * @brief Get field chan7_raw from rc_channels message - * - * @return RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan7_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field chan8_raw from rc_channels message - * - * @return RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan8_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 18); -} - -/** - * @brief Get field chan9_raw from rc_channels message - * - * @return RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan9_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 20); -} - -/** - * @brief Get field chan10_raw from rc_channels message - * - * @return RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan10_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 22); -} - -/** - * @brief Get field chan11_raw from rc_channels message - * - * @return RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan11_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 24); -} - -/** - * @brief Get field chan12_raw from rc_channels message - * - * @return RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan12_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 26); -} - -/** - * @brief Get field chan13_raw from rc_channels message - * - * @return RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan13_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 28); -} - -/** - * @brief Get field chan14_raw from rc_channels message - * - * @return RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan14_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 30); -} - -/** - * @brief Get field chan15_raw from rc_channels message - * - * @return RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan15_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 32); -} - -/** - * @brief Get field chan16_raw from rc_channels message - * - * @return RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan16_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 34); -} - -/** - * @brief Get field chan17_raw from rc_channels message - * - * @return RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan17_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 36); -} - -/** - * @brief Get field chan18_raw from rc_channels message - * - * @return RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_get_chan18_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 38); -} - -/** - * @brief Get field rssi from rc_channels message - * - * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. - */ -static inline uint8_t mavlink_msg_rc_channels_get_rssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 41); -} - -/** - * @brief Decode a rc_channels message into a struct - * - * @param msg The message to decode - * @param rc_channels C-struct to decode the message contents into - */ -static inline void mavlink_msg_rc_channels_decode(const mavlink_message_t* msg, mavlink_rc_channels_t* rc_channels) -{ -#if MAVLINK_NEED_BYTE_SWAP - rc_channels->time_boot_ms = mavlink_msg_rc_channels_get_time_boot_ms(msg); - rc_channels->chan1_raw = mavlink_msg_rc_channels_get_chan1_raw(msg); - rc_channels->chan2_raw = mavlink_msg_rc_channels_get_chan2_raw(msg); - rc_channels->chan3_raw = mavlink_msg_rc_channels_get_chan3_raw(msg); - rc_channels->chan4_raw = mavlink_msg_rc_channels_get_chan4_raw(msg); - rc_channels->chan5_raw = mavlink_msg_rc_channels_get_chan5_raw(msg); - rc_channels->chan6_raw = mavlink_msg_rc_channels_get_chan6_raw(msg); - rc_channels->chan7_raw = mavlink_msg_rc_channels_get_chan7_raw(msg); - rc_channels->chan8_raw = mavlink_msg_rc_channels_get_chan8_raw(msg); - rc_channels->chan9_raw = mavlink_msg_rc_channels_get_chan9_raw(msg); - rc_channels->chan10_raw = mavlink_msg_rc_channels_get_chan10_raw(msg); - rc_channels->chan11_raw = mavlink_msg_rc_channels_get_chan11_raw(msg); - rc_channels->chan12_raw = mavlink_msg_rc_channels_get_chan12_raw(msg); - rc_channels->chan13_raw = mavlink_msg_rc_channels_get_chan13_raw(msg); - rc_channels->chan14_raw = mavlink_msg_rc_channels_get_chan14_raw(msg); - rc_channels->chan15_raw = mavlink_msg_rc_channels_get_chan15_raw(msg); - rc_channels->chan16_raw = mavlink_msg_rc_channels_get_chan16_raw(msg); - rc_channels->chan17_raw = mavlink_msg_rc_channels_get_chan17_raw(msg); - rc_channels->chan18_raw = mavlink_msg_rc_channels_get_chan18_raw(msg); - rc_channels->chancount = mavlink_msg_rc_channels_get_chancount(msg); - rc_channels->rssi = mavlink_msg_rc_channels_get_rssi(msg); -#else - memcpy(rc_channels, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h deleted file mode 100644 index 15d4c6f95..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h +++ /dev/null @@ -1,425 +0,0 @@ -// MESSAGE RC_CHANNELS_OVERRIDE PACKING - -#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70 - -typedef struct __mavlink_rc_channels_override_t -{ - uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. - uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. - uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. - uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. - uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. - uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. - uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. - uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_rc_channels_override_t; - -#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 18 -#define MAVLINK_MSG_ID_70_LEN 18 - -#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC 124 -#define MAVLINK_MSG_ID_70_CRC 124 - - - -#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE { \ - "RC_CHANNELS_OVERRIDE", \ - 10, \ - { { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_rc_channels_override_t, chan1_raw) }, \ - { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_rc_channels_override_t, chan2_raw) }, \ - { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_override_t, chan3_raw) }, \ - { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_override_t, chan4_raw) }, \ - { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_override_t, chan5_raw) }, \ - { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_override_t, chan6_raw) }, \ - { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_override_t, chan7_raw) }, \ - { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_override_t, chan8_raw) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_override_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rc_channels_override_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a rc_channels_override message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; - _mav_put_uint16_t(buf, 0, chan1_raw); - _mav_put_uint16_t(buf, 2, chan2_raw); - _mav_put_uint16_t(buf, 4, chan3_raw); - _mav_put_uint16_t(buf, 6, chan4_raw); - _mav_put_uint16_t(buf, 8, chan5_raw); - _mav_put_uint16_t(buf, 10, chan6_raw); - _mav_put_uint16_t(buf, 12, chan7_raw); - _mav_put_uint16_t(buf, 14, chan8_raw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); -#else - mavlink_rc_channels_override_t packet; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); -#endif -} - -/** - * @brief Pack a rc_channels_override message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; - _mav_put_uint16_t(buf, 0, chan1_raw); - _mav_put_uint16_t(buf, 2, chan2_raw); - _mav_put_uint16_t(buf, 4, chan3_raw); - _mav_put_uint16_t(buf, 6, chan4_raw); - _mav_put_uint16_t(buf, 8, chan5_raw); - _mav_put_uint16_t(buf, 10, chan6_raw); - _mav_put_uint16_t(buf, 12, chan7_raw); - _mav_put_uint16_t(buf, 14, chan8_raw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); -#else - mavlink_rc_channels_override_t packet; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); -#endif -} - -/** - * @brief Encode a rc_channels_override struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param rc_channels_override C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) -{ - return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); -} - -/** - * @brief Encode a rc_channels_override struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rc_channels_override C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rc_channels_override_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) -{ - return mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, chan, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); -} - -/** - * @brief Send a rc_channels_override message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. - * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; - _mav_put_uint16_t(buf, 0, chan1_raw); - _mav_put_uint16_t(buf, 2, chan2_raw); - _mav_put_uint16_t(buf, 4, chan3_raw); - _mav_put_uint16_t(buf, 6, chan4_raw); - _mav_put_uint16_t(buf, 8, chan5_raw); - _mav_put_uint16_t(buf, 10, chan6_raw); - _mav_put_uint16_t(buf, 12, chan7_raw); - _mav_put_uint16_t(buf, 14, chan8_raw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); -#endif -#else - mavlink_rc_channels_override_t packet; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, chan1_raw); - _mav_put_uint16_t(buf, 2, chan2_raw); - _mav_put_uint16_t(buf, 4, chan3_raw); - _mav_put_uint16_t(buf, 6, chan4_raw); - _mav_put_uint16_t(buf, 8, chan5_raw); - _mav_put_uint16_t(buf, 10, chan6_raw); - _mav_put_uint16_t(buf, 12, chan7_raw); - _mav_put_uint16_t(buf, 14, chan8_raw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); -#endif -#else - mavlink_rc_channels_override_t *packet = (mavlink_rc_channels_override_t *)msgbuf; - packet->chan1_raw = chan1_raw; - packet->chan2_raw = chan2_raw; - packet->chan3_raw = chan3_raw; - packet->chan4_raw = chan4_raw; - packet->chan5_raw = chan5_raw; - packet->chan6_raw = chan6_raw; - packet->chan7_raw = chan7_raw; - packet->chan8_raw = chan8_raw; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE RC_CHANNELS_OVERRIDE UNPACKING - - -/** - * @brief Get field target_system from rc_channels_override message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from rc_channels_override message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field chan1_raw from rc_channels_override message - * - * @return RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. - */ -static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field chan2_raw from rc_channels_override message - * - * @return RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. - */ -static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field chan3_raw from rc_channels_override message - * - * @return RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. - */ -static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field chan4_raw from rc_channels_override message - * - * @return RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. - */ -static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field chan5_raw from rc_channels_override message - * - * @return RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. - */ -static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field chan6_raw from rc_channels_override message - * - * @return RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. - */ -static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Get field chan7_raw from rc_channels_override message - * - * @return RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. - */ -static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field chan8_raw from rc_channels_override message - * - * @return RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. - */ -static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 14); -} - -/** - * @brief Decode a rc_channels_override message into a struct - * - * @param msg The message to decode - * @param rc_channels_override C-struct to decode the message contents into - */ -static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message_t* msg, mavlink_rc_channels_override_t* rc_channels_override) -{ -#if MAVLINK_NEED_BYTE_SWAP - rc_channels_override->chan1_raw = mavlink_msg_rc_channels_override_get_chan1_raw(msg); - rc_channels_override->chan2_raw = mavlink_msg_rc_channels_override_get_chan2_raw(msg); - rc_channels_override->chan3_raw = mavlink_msg_rc_channels_override_get_chan3_raw(msg); - rc_channels_override->chan4_raw = mavlink_msg_rc_channels_override_get_chan4_raw(msg); - rc_channels_override->chan5_raw = mavlink_msg_rc_channels_override_get_chan5_raw(msg); - rc_channels_override->chan6_raw = mavlink_msg_rc_channels_override_get_chan6_raw(msg); - rc_channels_override->chan7_raw = mavlink_msg_rc_channels_override_get_chan7_raw(msg); - rc_channels_override->chan8_raw = mavlink_msg_rc_channels_override_get_chan8_raw(msg); - rc_channels_override->target_system = mavlink_msg_rc_channels_override_get_target_system(msg); - rc_channels_override->target_component = mavlink_msg_rc_channels_override_get_target_component(msg); -#else - memcpy(rc_channels_override, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h deleted file mode 100644 index d75a5934a..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h +++ /dev/null @@ -1,449 +0,0 @@ -// MESSAGE RC_CHANNELS_RAW PACKING - -#define MAVLINK_MSG_ID_RC_CHANNELS_RAW 35 - -typedef struct __mavlink_rc_channels_raw_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. -} mavlink_rc_channels_raw_t; - -#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN 22 -#define MAVLINK_MSG_ID_35_LEN 22 - -#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC 244 -#define MAVLINK_MSG_ID_35_CRC 244 - - - -#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW { \ - "RC_CHANNELS_RAW", \ - 11, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_raw_t, time_boot_ms) }, \ - { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_raw_t, chan1_raw) }, \ - { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_raw_t, chan2_raw) }, \ - { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_raw_t, chan3_raw) }, \ - { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_raw_t, chan4_raw) }, \ - { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_raw_t, chan5_raw) }, \ - { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_raw_t, chan6_raw) }, \ - { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_rc_channels_raw_t, chan7_raw) }, \ - { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_raw_t, chan8_raw) }, \ - { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_rc_channels_raw_t, port) }, \ - { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_rc_channels_raw_t, rssi) }, \ - } \ -} - - -/** - * @brief Pack a rc_channels_raw message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t port, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, chan1_raw); - _mav_put_uint16_t(buf, 6, chan2_raw); - _mav_put_uint16_t(buf, 8, chan3_raw); - _mav_put_uint16_t(buf, 10, chan4_raw); - _mav_put_uint16_t(buf, 12, chan5_raw); - _mav_put_uint16_t(buf, 14, chan6_raw); - _mav_put_uint16_t(buf, 16, chan7_raw); - _mav_put_uint16_t(buf, 18, chan8_raw); - _mav_put_uint8_t(buf, 20, port); - _mav_put_uint8_t(buf, 21, rssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); -#else - mavlink_rc_channels_raw_t packet; - packet.time_boot_ms = time_boot_ms; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - packet.port = port; - packet.rssi = rssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); -#endif -} - -/** - * @brief Pack a rc_channels_raw message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t port,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, chan1_raw); - _mav_put_uint16_t(buf, 6, chan2_raw); - _mav_put_uint16_t(buf, 8, chan3_raw); - _mav_put_uint16_t(buf, 10, chan4_raw); - _mav_put_uint16_t(buf, 12, chan5_raw); - _mav_put_uint16_t(buf, 14, chan6_raw); - _mav_put_uint16_t(buf, 16, chan7_raw); - _mav_put_uint16_t(buf, 18, chan8_raw); - _mav_put_uint8_t(buf, 20, port); - _mav_put_uint8_t(buf, 21, rssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); -#else - mavlink_rc_channels_raw_t packet; - packet.time_boot_ms = time_boot_ms; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - packet.port = port; - packet.rssi = rssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); -#endif -} - -/** - * @brief Encode a rc_channels_raw struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param rc_channels_raw C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_raw_t* rc_channels_raw) -{ - return mavlink_msg_rc_channels_raw_pack(system_id, component_id, msg, rc_channels_raw->time_boot_ms, rc_channels_raw->port, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); -} - -/** - * @brief Encode a rc_channels_raw struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rc_channels_raw C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rc_channels_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_raw_t* rc_channels_raw) -{ - return mavlink_msg_rc_channels_raw_pack_chan(system_id, component_id, chan, msg, rc_channels_raw->time_boot_ms, rc_channels_raw->port, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); -} - -/** - * @brief Send a rc_channels_raw message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, chan1_raw); - _mav_put_uint16_t(buf, 6, chan2_raw); - _mav_put_uint16_t(buf, 8, chan3_raw); - _mav_put_uint16_t(buf, 10, chan4_raw); - _mav_put_uint16_t(buf, 12, chan5_raw); - _mav_put_uint16_t(buf, 14, chan6_raw); - _mav_put_uint16_t(buf, 16, chan7_raw); - _mav_put_uint16_t(buf, 18, chan8_raw); - _mav_put_uint8_t(buf, 20, port); - _mav_put_uint8_t(buf, 21, rssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); -#endif -#else - mavlink_rc_channels_raw_t packet; - packet.time_boot_ms = time_boot_ms; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - packet.port = port; - packet.rssi = rssi; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_rc_channels_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, chan1_raw); - _mav_put_uint16_t(buf, 6, chan2_raw); - _mav_put_uint16_t(buf, 8, chan3_raw); - _mav_put_uint16_t(buf, 10, chan4_raw); - _mav_put_uint16_t(buf, 12, chan5_raw); - _mav_put_uint16_t(buf, 14, chan6_raw); - _mav_put_uint16_t(buf, 16, chan7_raw); - _mav_put_uint16_t(buf, 18, chan8_raw); - _mav_put_uint8_t(buf, 20, port); - _mav_put_uint8_t(buf, 21, rssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); -#endif -#else - mavlink_rc_channels_raw_t *packet = (mavlink_rc_channels_raw_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->chan1_raw = chan1_raw; - packet->chan2_raw = chan2_raw; - packet->chan3_raw = chan3_raw; - packet->chan4_raw = chan4_raw; - packet->chan5_raw = chan5_raw; - packet->chan6_raw = chan6_raw; - packet->chan7_raw = chan7_raw; - packet->chan8_raw = chan8_raw; - packet->port = port; - packet->rssi = rssi; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE RC_CHANNELS_RAW UNPACKING - - -/** - * @brief Get field time_boot_ms from rc_channels_raw message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_rc_channels_raw_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field port from rc_channels_raw message - * - * @return Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - */ -static inline uint8_t mavlink_msg_rc_channels_raw_get_port(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 20); -} - -/** - * @brief Get field chan1_raw from rc_channels_raw message - * - * @return RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field chan2_raw from rc_channels_raw message - * - * @return RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field chan3_raw from rc_channels_raw message - * - * @return RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field chan4_raw from rc_channels_raw message - * - * @return RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Get field chan5_raw from rc_channels_raw message - * - * @return RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field chan6_raw from rc_channels_raw message - * - * @return RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 14); -} - -/** - * @brief Get field chan7_raw from rc_channels_raw message - * - * @return RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field chan8_raw from rc_channels_raw message - * - * @return RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - */ -static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 18); -} - -/** - * @brief Get field rssi from rc_channels_raw message - * - * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. - */ -static inline uint8_t mavlink_msg_rc_channels_raw_get_rssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 21); -} - -/** - * @brief Decode a rc_channels_raw message into a struct - * - * @param msg The message to decode - * @param rc_channels_raw C-struct to decode the message contents into - */ -static inline void mavlink_msg_rc_channels_raw_decode(const mavlink_message_t* msg, mavlink_rc_channels_raw_t* rc_channels_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP - rc_channels_raw->time_boot_ms = mavlink_msg_rc_channels_raw_get_time_boot_ms(msg); - rc_channels_raw->chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(msg); - rc_channels_raw->chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(msg); - rc_channels_raw->chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(msg); - rc_channels_raw->chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(msg); - rc_channels_raw->chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(msg); - rc_channels_raw->chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(msg); - rc_channels_raw->chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(msg); - rc_channels_raw->chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(msg); - rc_channels_raw->port = mavlink_msg_rc_channels_raw_get_port(msg); - rc_channels_raw->rssi = mavlink_msg_rc_channels_raw_get_rssi(msg); -#else - memcpy(rc_channels_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h deleted file mode 100644 index f400f987d..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h +++ /dev/null @@ -1,449 +0,0 @@ -// MESSAGE RC_CHANNELS_SCALED PACKING - -#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED 34 - -typedef struct __mavlink_rc_channels_scaled_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. -} mavlink_rc_channels_scaled_t; - -#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN 22 -#define MAVLINK_MSG_ID_34_LEN 22 - -#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC 237 -#define MAVLINK_MSG_ID_34_CRC 237 - - - -#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED { \ - "RC_CHANNELS_SCALED", \ - 11, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_scaled_t, time_boot_ms) }, \ - { "chan1_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_rc_channels_scaled_t, chan1_scaled) }, \ - { "chan2_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_rc_channels_scaled_t, chan2_scaled) }, \ - { "chan3_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_rc_channels_scaled_t, chan3_scaled) }, \ - { "chan4_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_rc_channels_scaled_t, chan4_scaled) }, \ - { "chan5_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_rc_channels_scaled_t, chan5_scaled) }, \ - { "chan6_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_rc_channels_scaled_t, chan6_scaled) }, \ - { "chan7_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_rc_channels_scaled_t, chan7_scaled) }, \ - { "chan8_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_rc_channels_scaled_t, chan8_scaled) }, \ - { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_rc_channels_scaled_t, port) }, \ - { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_rc_channels_scaled_t, rssi) }, \ - } \ -} - - -/** - * @brief Pack a rc_channels_scaled message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t port, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, chan1_scaled); - _mav_put_int16_t(buf, 6, chan2_scaled); - _mav_put_int16_t(buf, 8, chan3_scaled); - _mav_put_int16_t(buf, 10, chan4_scaled); - _mav_put_int16_t(buf, 12, chan5_scaled); - _mav_put_int16_t(buf, 14, chan6_scaled); - _mav_put_int16_t(buf, 16, chan7_scaled); - _mav_put_int16_t(buf, 18, chan8_scaled); - _mav_put_uint8_t(buf, 20, port); - _mav_put_uint8_t(buf, 21, rssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); -#else - mavlink_rc_channels_scaled_t packet; - packet.time_boot_ms = time_boot_ms; - packet.chan1_scaled = chan1_scaled; - packet.chan2_scaled = chan2_scaled; - packet.chan3_scaled = chan3_scaled; - packet.chan4_scaled = chan4_scaled; - packet.chan5_scaled = chan5_scaled; - packet.chan6_scaled = chan6_scaled; - packet.chan7_scaled = chan7_scaled; - packet.chan8_scaled = chan8_scaled; - packet.port = port; - packet.rssi = rssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); -#endif -} - -/** - * @brief Pack a rc_channels_scaled message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t port,int16_t chan1_scaled,int16_t chan2_scaled,int16_t chan3_scaled,int16_t chan4_scaled,int16_t chan5_scaled,int16_t chan6_scaled,int16_t chan7_scaled,int16_t chan8_scaled,uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, chan1_scaled); - _mav_put_int16_t(buf, 6, chan2_scaled); - _mav_put_int16_t(buf, 8, chan3_scaled); - _mav_put_int16_t(buf, 10, chan4_scaled); - _mav_put_int16_t(buf, 12, chan5_scaled); - _mav_put_int16_t(buf, 14, chan6_scaled); - _mav_put_int16_t(buf, 16, chan7_scaled); - _mav_put_int16_t(buf, 18, chan8_scaled); - _mav_put_uint8_t(buf, 20, port); - _mav_put_uint8_t(buf, 21, rssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); -#else - mavlink_rc_channels_scaled_t packet; - packet.time_boot_ms = time_boot_ms; - packet.chan1_scaled = chan1_scaled; - packet.chan2_scaled = chan2_scaled; - packet.chan3_scaled = chan3_scaled; - packet.chan4_scaled = chan4_scaled; - packet.chan5_scaled = chan5_scaled; - packet.chan6_scaled = chan6_scaled; - packet.chan7_scaled = chan7_scaled; - packet.chan8_scaled = chan8_scaled; - packet.port = port; - packet.rssi = rssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); -#endif -} - -/** - * @brief Encode a rc_channels_scaled struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param rc_channels_scaled C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_scaled_t* rc_channels_scaled) -{ - return mavlink_msg_rc_channels_scaled_pack(system_id, component_id, msg, rc_channels_scaled->time_boot_ms, rc_channels_scaled->port, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); -} - -/** - * @brief Encode a rc_channels_scaled struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rc_channels_scaled C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_rc_channels_scaled_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_scaled_t* rc_channels_scaled) -{ - return mavlink_msg_rc_channels_scaled_pack_chan(system_id, component_id, chan, msg, rc_channels_scaled->time_boot_ms, rc_channels_scaled->port, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); -} - -/** - * @brief Send a rc_channels_scaled message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, chan1_scaled); - _mav_put_int16_t(buf, 6, chan2_scaled); - _mav_put_int16_t(buf, 8, chan3_scaled); - _mav_put_int16_t(buf, 10, chan4_scaled); - _mav_put_int16_t(buf, 12, chan5_scaled); - _mav_put_int16_t(buf, 14, chan6_scaled); - _mav_put_int16_t(buf, 16, chan7_scaled); - _mav_put_int16_t(buf, 18, chan8_scaled); - _mav_put_uint8_t(buf, 20, port); - _mav_put_uint8_t(buf, 21, rssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); -#endif -#else - mavlink_rc_channels_scaled_t packet; - packet.time_boot_ms = time_boot_ms; - packet.chan1_scaled = chan1_scaled; - packet.chan2_scaled = chan2_scaled; - packet.chan3_scaled = chan3_scaled; - packet.chan4_scaled = chan4_scaled; - packet.chan5_scaled = chan5_scaled; - packet.chan6_scaled = chan6_scaled; - packet.chan7_scaled = chan7_scaled; - packet.chan8_scaled = chan8_scaled; - packet.port = port; - packet.rssi = rssi; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_rc_channels_scaled_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, chan1_scaled); - _mav_put_int16_t(buf, 6, chan2_scaled); - _mav_put_int16_t(buf, 8, chan3_scaled); - _mav_put_int16_t(buf, 10, chan4_scaled); - _mav_put_int16_t(buf, 12, chan5_scaled); - _mav_put_int16_t(buf, 14, chan6_scaled); - _mav_put_int16_t(buf, 16, chan7_scaled); - _mav_put_int16_t(buf, 18, chan8_scaled); - _mav_put_uint8_t(buf, 20, port); - _mav_put_uint8_t(buf, 21, rssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); -#endif -#else - mavlink_rc_channels_scaled_t *packet = (mavlink_rc_channels_scaled_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->chan1_scaled = chan1_scaled; - packet->chan2_scaled = chan2_scaled; - packet->chan3_scaled = chan3_scaled; - packet->chan4_scaled = chan4_scaled; - packet->chan5_scaled = chan5_scaled; - packet->chan6_scaled = chan6_scaled; - packet->chan7_scaled = chan7_scaled; - packet->chan8_scaled = chan8_scaled; - packet->port = port; - packet->rssi = rssi; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE RC_CHANNELS_SCALED UNPACKING - - -/** - * @brief Get field time_boot_ms from rc_channels_scaled message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_rc_channels_scaled_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field port from rc_channels_scaled message - * - * @return Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - */ -static inline uint8_t mavlink_msg_rc_channels_scaled_get_port(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 20); -} - -/** - * @brief Get field chan1_scaled from rc_channels_scaled message - * - * @return RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - */ -static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 4); -} - -/** - * @brief Get field chan2_scaled from rc_channels_scaled message - * - * @return RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - */ -static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 6); -} - -/** - * @brief Get field chan3_scaled from rc_channels_scaled message - * - * @return RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - */ -static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field chan4_scaled from rc_channels_scaled message - * - * @return RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - */ -static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 10); -} - -/** - * @brief Get field chan5_scaled from rc_channels_scaled message - * - * @return RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - */ -static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Get field chan6_scaled from rc_channels_scaled message - * - * @return RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - */ -static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 14); -} - -/** - * @brief Get field chan7_scaled from rc_channels_scaled message - * - * @return RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - */ -static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 16); -} - -/** - * @brief Get field chan8_scaled from rc_channels_scaled message - * - * @return RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. - */ -static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 18); -} - -/** - * @brief Get field rssi from rc_channels_scaled message - * - * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. - */ -static inline uint8_t mavlink_msg_rc_channels_scaled_get_rssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 21); -} - -/** - * @brief Decode a rc_channels_scaled message into a struct - * - * @param msg The message to decode - * @param rc_channels_scaled C-struct to decode the message contents into - */ -static inline void mavlink_msg_rc_channels_scaled_decode(const mavlink_message_t* msg, mavlink_rc_channels_scaled_t* rc_channels_scaled) -{ -#if MAVLINK_NEED_BYTE_SWAP - rc_channels_scaled->time_boot_ms = mavlink_msg_rc_channels_scaled_get_time_boot_ms(msg); - rc_channels_scaled->chan1_scaled = mavlink_msg_rc_channels_scaled_get_chan1_scaled(msg); - rc_channels_scaled->chan2_scaled = mavlink_msg_rc_channels_scaled_get_chan2_scaled(msg); - rc_channels_scaled->chan3_scaled = mavlink_msg_rc_channels_scaled_get_chan3_scaled(msg); - rc_channels_scaled->chan4_scaled = mavlink_msg_rc_channels_scaled_get_chan4_scaled(msg); - rc_channels_scaled->chan5_scaled = mavlink_msg_rc_channels_scaled_get_chan5_scaled(msg); - rc_channels_scaled->chan6_scaled = mavlink_msg_rc_channels_scaled_get_chan6_scaled(msg); - rc_channels_scaled->chan7_scaled = mavlink_msg_rc_channels_scaled_get_chan7_scaled(msg); - rc_channels_scaled->chan8_scaled = mavlink_msg_rc_channels_scaled_get_chan8_scaled(msg); - rc_channels_scaled->port = mavlink_msg_rc_channels_scaled_get_port(msg); - rc_channels_scaled->rssi = mavlink_msg_rc_channels_scaled_get_rssi(msg); -#else - memcpy(rc_channels_scaled, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h deleted file mode 100644 index 2ebcc54da..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE REQUEST_DATA_STREAM PACKING - -#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM 66 - -typedef struct __mavlink_request_data_stream_t -{ - uint16_t req_message_rate; ///< The requested interval between two messages of this type - uint8_t target_system; ///< The target requested to send the message stream. - uint8_t target_component; ///< The target requested to send the message stream. - uint8_t req_stream_id; ///< The ID of the requested data stream - uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. -} mavlink_request_data_stream_t; - -#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN 6 -#define MAVLINK_MSG_ID_66_LEN 6 - -#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC 148 -#define MAVLINK_MSG_ID_66_CRC 148 - - - -#define MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM { \ - "REQUEST_DATA_STREAM", \ - 5, \ - { { "req_message_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_request_data_stream_t, req_message_rate) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_request_data_stream_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_request_data_stream_t, target_component) }, \ - { "req_stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_request_data_stream_t, req_stream_id) }, \ - { "start_stop", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_request_data_stream_t, start_stop) }, \ - } \ -} - - -/** - * @brief Pack a request_data_stream message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system The target requested to send the message stream. - * @param target_component The target requested to send the message stream. - * @param req_stream_id The ID of the requested data stream - * @param req_message_rate The requested interval between two messages of this type - * @param start_stop 1 to start sending, 0 to stop sending. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN]; - _mav_put_uint16_t(buf, 0, req_message_rate); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_uint8_t(buf, 4, req_stream_id); - _mav_put_uint8_t(buf, 5, start_stop); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); -#else - mavlink_request_data_stream_t packet; - packet.req_message_rate = req_message_rate; - packet.target_system = target_system; - packet.target_component = target_component; - packet.req_stream_id = req_stream_id; - packet.start_stop = start_stop; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); -#endif -} - -/** - * @brief Pack a request_data_stream message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system The target requested to send the message stream. - * @param target_component The target requested to send the message stream. - * @param req_stream_id The ID of the requested data stream - * @param req_message_rate The requested interval between two messages of this type - * @param start_stop 1 to start sending, 0 to stop sending. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t req_stream_id,uint16_t req_message_rate,uint8_t start_stop) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN]; - _mav_put_uint16_t(buf, 0, req_message_rate); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_uint8_t(buf, 4, req_stream_id); - _mav_put_uint8_t(buf, 5, start_stop); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); -#else - mavlink_request_data_stream_t packet; - packet.req_message_rate = req_message_rate; - packet.target_system = target_system; - packet.target_component = target_component; - packet.req_stream_id = req_stream_id; - packet.start_stop = start_stop; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); -#endif -} - -/** - * @brief Encode a request_data_stream struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param request_data_stream C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_data_stream_t* request_data_stream) -{ - return mavlink_msg_request_data_stream_pack(system_id, component_id, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); -} - -/** - * @brief Encode a request_data_stream struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param request_data_stream C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_request_data_stream_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_request_data_stream_t* request_data_stream) -{ - return mavlink_msg_request_data_stream_pack_chan(system_id, component_id, chan, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); -} - -/** - * @brief Send a request_data_stream message - * @param chan MAVLink channel to send the message - * - * @param target_system The target requested to send the message stream. - * @param target_component The target requested to send the message stream. - * @param req_stream_id The ID of the requested data stream - * @param req_message_rate The requested interval between two messages of this type - * @param start_stop 1 to start sending, 0 to stop sending. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN]; - _mav_put_uint16_t(buf, 0, req_message_rate); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_uint8_t(buf, 4, req_stream_id); - _mav_put_uint8_t(buf, 5, start_stop); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); -#endif -#else - mavlink_request_data_stream_t packet; - packet.req_message_rate = req_message_rate; - packet.target_system = target_system; - packet.target_component = target_component; - packet.req_stream_id = req_stream_id; - packet.start_stop = start_stop; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)&packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)&packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_request_data_stream_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, req_message_rate); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_uint8_t(buf, 4, req_stream_id); - _mav_put_uint8_t(buf, 5, start_stop); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); -#endif -#else - mavlink_request_data_stream_t *packet = (mavlink_request_data_stream_t *)msgbuf; - packet->req_message_rate = req_message_rate; - packet->target_system = target_system; - packet->target_component = target_component; - packet->req_stream_id = req_stream_id; - packet->start_stop = start_stop; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE REQUEST_DATA_STREAM UNPACKING - - -/** - * @brief Get field target_system from request_data_stream message - * - * @return The target requested to send the message stream. - */ -static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from request_data_stream message - * - * @return The target requested to send the message stream. - */ -static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field req_stream_id from request_data_stream message - * - * @return The ID of the requested data stream - */ -static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field req_message_rate from request_data_stream message - * - * @return The requested interval between two messages of this type - */ -static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field start_stop from request_data_stream message - * - * @return 1 to start sending, 0 to stop sending. - */ -static inline uint8_t mavlink_msg_request_data_stream_get_start_stop(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Decode a request_data_stream message into a struct - * - * @param msg The message to decode - * @param request_data_stream C-struct to decode the message contents into - */ -static inline void mavlink_msg_request_data_stream_decode(const mavlink_message_t* msg, mavlink_request_data_stream_t* request_data_stream) -{ -#if MAVLINK_NEED_BYTE_SWAP - request_data_stream->req_message_rate = mavlink_msg_request_data_stream_get_req_message_rate(msg); - request_data_stream->target_system = mavlink_msg_request_data_stream_get_target_system(msg); - request_data_stream->target_component = mavlink_msg_request_data_stream_get_target_component(msg); - request_data_stream->req_stream_id = mavlink_msg_request_data_stream_get_req_stream_id(msg); - request_data_stream->start_stop = mavlink_msg_request_data_stream_get_start_stop(msg); -#else - memcpy(request_data_stream, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h deleted file mode 100644 index 131b756b6..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE ROLL_PITCH_YAW_RATES_THRUST_SETPOINT PACKING - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT 80 - -typedef struct __mavlink_roll_pitch_yaw_rates_thrust_setpoint_t -{ - uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot - float roll_rate; ///< Desired roll rate in radians per second - float pitch_rate; ///< Desired pitch rate in radians per second - float yaw_rate; ///< Desired yaw rate in radians per second - float thrust; ///< Collective thrust, normalized to 0 .. 1 -} mavlink_roll_pitch_yaw_rates_thrust_setpoint_t; - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN 20 -#define MAVLINK_MSG_ID_80_LEN 20 - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC 127 -#define MAVLINK_MSG_ID_80_CRC 127 - - - -#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT { \ - "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT", \ - 5, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, time_boot_ms) }, \ - { "roll_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, roll_rate) }, \ - { "pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, pitch_rate) }, \ - { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, yaw_rate) }, \ - { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, thrust) }, \ - } \ -} - - -/** - * @brief Pack a roll_pitch_yaw_rates_thrust_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll_rate Desired roll rate in radians per second - * @param pitch_rate Desired pitch rate in radians per second - * @param yaw_rate Desired yaw rate in radians per second - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float roll_rate, float pitch_rate, float yaw_rate, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_rate); - _mav_put_float(buf, 8, pitch_rate); - _mav_put_float(buf, 12, yaw_rate); - _mav_put_float(buf, 16, thrust); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#else - mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll_rate = roll_rate; - packet.pitch_rate = pitch_rate; - packet.yaw_rate = yaw_rate; - packet.thrust = thrust; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a roll_pitch_yaw_rates_thrust_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll_rate Desired roll rate in radians per second - * @param pitch_rate Desired pitch rate in radians per second - * @param yaw_rate Desired yaw rate in radians per second - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,float roll_rate,float pitch_rate,float yaw_rate,float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_rate); - _mav_put_float(buf, 8, pitch_rate); - _mav_put_float(buf, 12, yaw_rate); - _mav_put_float(buf, 16, thrust); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#else - mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll_rate = roll_rate; - packet.pitch_rate = pitch_rate; - packet.yaw_rate = yaw_rate; - packet.thrust = thrust; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param roll_pitch_yaw_rates_thrust_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_rates_thrust_setpoint_t* roll_pitch_yaw_rates_thrust_setpoint) -{ - return mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms, roll_pitch_yaw_rates_thrust_setpoint->roll_rate, roll_pitch_yaw_rates_thrust_setpoint->pitch_rate, roll_pitch_yaw_rates_thrust_setpoint->yaw_rate, roll_pitch_yaw_rates_thrust_setpoint->thrust); -} - -/** - * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param roll_pitch_yaw_rates_thrust_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_rates_thrust_setpoint_t* roll_pitch_yaw_rates_thrust_setpoint) -{ - return mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms, roll_pitch_yaw_rates_thrust_setpoint->roll_rate, roll_pitch_yaw_rates_thrust_setpoint->pitch_rate, roll_pitch_yaw_rates_thrust_setpoint->yaw_rate, roll_pitch_yaw_rates_thrust_setpoint->thrust); -} - -/** - * @brief Send a roll_pitch_yaw_rates_thrust_setpoint message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll_rate Desired roll rate in radians per second - * @param pitch_rate Desired pitch rate in radians per second - * @param yaw_rate Desired yaw rate in radians per second - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll_rate, float pitch_rate, float yaw_rate, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_rate); - _mav_put_float(buf, 8, pitch_rate); - _mav_put_float(buf, 12, yaw_rate); - _mav_put_float(buf, 16, thrust); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif -#else - mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll_rate = roll_rate; - packet.pitch_rate = pitch_rate; - packet.yaw_rate = yaw_rate; - packet.thrust = thrust; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll_rate, float pitch_rate, float yaw_rate, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_rate); - _mav_put_float(buf, 8, pitch_rate); - _mav_put_float(buf, 12, yaw_rate); - _mav_put_float(buf, 16, thrust); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif -#else - mavlink_roll_pitch_yaw_rates_thrust_setpoint_t *packet = (mavlink_roll_pitch_yaw_rates_thrust_setpoint_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->roll_rate = roll_rate; - packet->pitch_rate = pitch_rate; - packet->yaw_rate = yaw_rate; - packet->thrust = thrust; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE ROLL_PITCH_YAW_RATES_THRUST_SETPOINT UNPACKING - - -/** - * @brief Get field time_boot_ms from roll_pitch_yaw_rates_thrust_setpoint message - * - * @return Timestamp in milliseconds since system boot - */ -static inline uint32_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field roll_rate from roll_pitch_yaw_rates_thrust_setpoint message - * - * @return Desired roll rate in radians per second - */ -static inline float mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_roll_rate(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field pitch_rate from roll_pitch_yaw_rates_thrust_setpoint message - * - * @return Desired pitch rate in radians per second - */ -static inline float mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_pitch_rate(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw_rate from roll_pitch_yaw_rates_thrust_setpoint message - * - * @return Desired yaw rate in radians per second - */ -static inline float mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_yaw_rate(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field thrust from roll_pitch_yaw_rates_thrust_setpoint message - * - * @return Collective thrust, normalized to 0 .. 1 - */ -static inline float mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a roll_pitch_yaw_rates_thrust_setpoint message into a struct - * - * @param msg The message to decode - * @param roll_pitch_yaw_rates_thrust_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_rates_thrust_setpoint_t* roll_pitch_yaw_rates_thrust_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_time_boot_ms(msg); - roll_pitch_yaw_rates_thrust_setpoint->roll_rate = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_roll_rate(msg); - roll_pitch_yaw_rates_thrust_setpoint->pitch_rate = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_pitch_rate(msg); - roll_pitch_yaw_rates_thrust_setpoint->yaw_rate = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_yaw_rate(msg); - roll_pitch_yaw_rates_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_thrust(msg); -#else - memcpy(roll_pitch_yaw_rates_thrust_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h deleted file mode 100644 index 757e6d138..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT PACKING - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT 59 - -typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t -{ - uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot - float roll_speed; ///< Desired roll angular speed in rad/s - float pitch_speed; ///< Desired pitch angular speed in rad/s - float yaw_speed; ///< Desired yaw angular speed in rad/s - float thrust; ///< Collective thrust, normalized to 0 .. 1 -} mavlink_roll_pitch_yaw_speed_thrust_setpoint_t; - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN 20 -#define MAVLINK_MSG_ID_59_LEN 20 - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC 238 -#define MAVLINK_MSG_ID_59_CRC 238 - - - -#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT { \ - "ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT", \ - 5, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, time_boot_ms) }, \ - { "roll_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, roll_speed) }, \ - { "pitch_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, pitch_speed) }, \ - { "yaw_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, yaw_speed) }, \ - { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, thrust) }, \ - } \ -} - - -/** - * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_speed); - _mav_put_float(buf, 8, pitch_speed); - _mav_put_float(buf, 12, yaw_speed); - _mav_put_float(buf, 16, thrust); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#else - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll_speed = roll_speed; - packet.pitch_speed = pitch_speed; - packet.yaw_speed = yaw_speed; - packet.thrust = thrust; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,float roll_speed,float pitch_speed,float yaw_speed,float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_speed); - _mav_put_float(buf, 8, pitch_speed); - _mav_put_float(buf, 12, yaw_speed); - _mav_put_float(buf, 16, thrust); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#else - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll_speed = roll_speed; - packet.pitch_speed = pitch_speed; - packet.yaw_speed = yaw_speed; - packet.thrust = thrust; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) -{ - return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust); -} - -/** - * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) -{ - return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust); -} - -/** - * @brief Send a roll_pitch_yaw_speed_thrust_setpoint message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_speed); - _mav_put_float(buf, 8, pitch_speed); - _mav_put_float(buf, 12, yaw_speed); - _mav_put_float(buf, 16, thrust); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif -#else - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll_speed = roll_speed; - packet.pitch_speed = pitch_speed; - packet.yaw_speed = yaw_speed; - packet.thrust = thrust; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll_speed); - _mav_put_float(buf, 8, pitch_speed); - _mav_put_float(buf, 12, yaw_speed); - _mav_put_float(buf, 16, thrust); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif -#else - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *packet = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->roll_speed = roll_speed; - packet->pitch_speed = pitch_speed; - packet->yaw_speed = yaw_speed; - packet->thrust = thrust; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT UNPACKING - - -/** - * @brief Get field time_boot_ms from roll_pitch_yaw_speed_thrust_setpoint message - * - * @return Timestamp in milliseconds since system boot - */ -static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field roll_speed from roll_pitch_yaw_speed_thrust_setpoint message - * - * @return Desired roll angular speed in rad/s - */ -static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field pitch_speed from roll_pitch_yaw_speed_thrust_setpoint message - * - * @return Desired pitch angular speed in rad/s - */ -static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw_speed from roll_pitch_yaw_speed_thrust_setpoint message - * - * @return Desired yaw angular speed in rad/s - */ -static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field thrust from roll_pitch_yaw_speed_thrust_setpoint message - * - * @return Collective thrust, normalized to 0 .. 1 - */ -static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a roll_pitch_yaw_speed_thrust_setpoint message into a struct - * - * @param msg The message to decode - * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_boot_ms(msg); - roll_pitch_yaw_speed_thrust_setpoint->roll_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(msg); - roll_pitch_yaw_speed_thrust_setpoint->pitch_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(msg); - roll_pitch_yaw_speed_thrust_setpoint->yaw_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(msg); - roll_pitch_yaw_speed_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(msg); -#else - memcpy(roll_pitch_yaw_speed_thrust_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h deleted file mode 100644 index f04eedcb3..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT PACKING - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT 58 - -typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t -{ - uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot - float roll; ///< Desired roll angle in radians - float pitch; ///< Desired pitch angle in radians - float yaw; ///< Desired yaw angle in radians - float thrust; ///< Collective thrust, normalized to 0 .. 1 -} mavlink_roll_pitch_yaw_thrust_setpoint_t; - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN 20 -#define MAVLINK_MSG_ID_58_LEN 20 - -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC 239 -#define MAVLINK_MSG_ID_58_CRC 239 - - - -#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT { \ - "ROLL_PITCH_YAW_THRUST_SETPOINT", \ - 5, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, time_boot_ms) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, yaw) }, \ - { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, thrust) }, \ - } \ -} - - -/** - * @brief Pack a roll_pitch_yaw_thrust_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, thrust); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#else - mavlink_roll_pitch_yaw_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a roll_pitch_yaw_thrust_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,float roll,float pitch,float yaw,float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, thrust); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#else - mavlink_roll_pitch_yaw_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a roll_pitch_yaw_thrust_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) -{ - return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_thrust_setpoint->time_boot_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); -} - -/** - * @brief Encode a roll_pitch_yaw_thrust_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) -{ - return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_thrust_setpoint->time_boot_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); -} - -/** - * @brief Send a roll_pitch_yaw_thrust_setpoint message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp in milliseconds since system boot - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, thrust); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif -#else - mavlink_roll_pitch_yaw_thrust_setpoint_t packet; - packet.time_boot_ms = time_boot_ms; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, roll); - _mav_put_float(buf, 8, pitch); - _mav_put_float(buf, 12, yaw); - _mav_put_float(buf, 16, thrust); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif -#else - mavlink_roll_pitch_yaw_thrust_setpoint_t *packet = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - packet->thrust = thrust; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT UNPACKING - - -/** - * @brief Get field time_boot_ms from roll_pitch_yaw_thrust_setpoint message - * - * @return Timestamp in milliseconds since system boot - */ -static inline uint32_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field roll from roll_pitch_yaw_thrust_setpoint message - * - * @return Desired roll angle in radians - */ -static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field pitch from roll_pitch_yaw_thrust_setpoint message - * - * @return Desired pitch angle in radians - */ -static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from roll_pitch_yaw_thrust_setpoint message - * - * @return Desired yaw angle in radians - */ -static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field thrust from roll_pitch_yaw_thrust_setpoint message - * - * @return Collective thrust, normalized to 0 .. 1 - */ -static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a roll_pitch_yaw_thrust_setpoint message into a struct - * - * @param msg The message to decode - * @param roll_pitch_yaw_thrust_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - roll_pitch_yaw_thrust_setpoint->time_boot_ms = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_boot_ms(msg); - roll_pitch_yaw_thrust_setpoint->roll = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(msg); - roll_pitch_yaw_thrust_setpoint->pitch = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(msg); - roll_pitch_yaw_thrust_setpoint->yaw = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(msg); - roll_pitch_yaw_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(msg); -#else - memcpy(roll_pitch_yaw_thrust_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h deleted file mode 100644 index 0176dfcc8..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE SAFETY_ALLOWED_AREA PACKING - -#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA 55 - -typedef struct __mavlink_safety_allowed_area_t -{ - float p1x; ///< x position 1 / Latitude 1 - float p1y; ///< y position 1 / Longitude 1 - float p1z; ///< z position 1 / Altitude 1 - float p2x; ///< x position 2 / Latitude 2 - float p2y; ///< y position 2 / Longitude 2 - float p2z; ///< z position 2 / Altitude 2 - uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. -} mavlink_safety_allowed_area_t; - -#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN 25 -#define MAVLINK_MSG_ID_55_LEN 25 - -#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC 3 -#define MAVLINK_MSG_ID_55_CRC 3 - - - -#define MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA { \ - "SAFETY_ALLOWED_AREA", \ - 7, \ - { { "p1x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_safety_allowed_area_t, p1x) }, \ - { "p1y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_safety_allowed_area_t, p1y) }, \ - { "p1z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_safety_allowed_area_t, p1z) }, \ - { "p2x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_safety_allowed_area_t, p2x) }, \ - { "p2y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_safety_allowed_area_t, p2y) }, \ - { "p2z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_safety_allowed_area_t, p2z) }, \ - { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_safety_allowed_area_t, frame) }, \ - } \ -} - - -/** - * @brief Pack a safety_allowed_area message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - * @param p1x x position 1 / Latitude 1 - * @param p1y y position 1 / Longitude 1 - * @param p1z z position 1 / Altitude 1 - * @param p2x x position 2 / Latitude 2 - * @param p2y y position 2 / Longitude 2 - * @param p2z z position 2 / Altitude 2 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN]; - _mav_put_float(buf, 0, p1x); - _mav_put_float(buf, 4, p1y); - _mav_put_float(buf, 8, p1z); - _mav_put_float(buf, 12, p2x); - _mav_put_float(buf, 16, p2y); - _mav_put_float(buf, 20, p2z); - _mav_put_uint8_t(buf, 24, frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); -#else - mavlink_safety_allowed_area_t packet; - packet.p1x = p1x; - packet.p1y = p1y; - packet.p1z = p1z; - packet.p2x = p2x; - packet.p2y = p2y; - packet.p2z = p2z; - packet.frame = frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); -#endif -} - -/** - * @brief Pack a safety_allowed_area message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - * @param p1x x position 1 / Latitude 1 - * @param p1y y position 1 / Longitude 1 - * @param p1z z position 1 / Altitude 1 - * @param p2x x position 2 / Latitude 2 - * @param p2y y position 2 / Longitude 2 - * @param p2z z position 2 / Altitude 2 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t frame,float p1x,float p1y,float p1z,float p2x,float p2y,float p2z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN]; - _mav_put_float(buf, 0, p1x); - _mav_put_float(buf, 4, p1y); - _mav_put_float(buf, 8, p1z); - _mav_put_float(buf, 12, p2x); - _mav_put_float(buf, 16, p2y); - _mav_put_float(buf, 20, p2z); - _mav_put_uint8_t(buf, 24, frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); -#else - mavlink_safety_allowed_area_t packet; - packet.p1x = p1x; - packet.p1y = p1y; - packet.p1z = p1z; - packet.p2x = p2x; - packet.p2y = p2y; - packet.p2z = p2z; - packet.frame = frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); -#endif -} - -/** - * @brief Encode a safety_allowed_area struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param safety_allowed_area C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_safety_allowed_area_t* safety_allowed_area) -{ - return mavlink_msg_safety_allowed_area_pack(system_id, component_id, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); -} - -/** - * @brief Encode a safety_allowed_area struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param safety_allowed_area C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_safety_allowed_area_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_safety_allowed_area_t* safety_allowed_area) -{ - return mavlink_msg_safety_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); -} - -/** - * @brief Send a safety_allowed_area message - * @param chan MAVLink channel to send the message - * - * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - * @param p1x x position 1 / Latitude 1 - * @param p1y y position 1 / Longitude 1 - * @param p1z z position 1 / Altitude 1 - * @param p2x x position 2 / Latitude 2 - * @param p2y y position 2 / Longitude 2 - * @param p2z z position 2 / Altitude 2 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN]; - _mav_put_float(buf, 0, p1x); - _mav_put_float(buf, 4, p1y); - _mav_put_float(buf, 8, p1z); - _mav_put_float(buf, 12, p2x); - _mav_put_float(buf, 16, p2y); - _mav_put_float(buf, 20, p2z); - _mav_put_uint8_t(buf, 24, frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); -#endif -#else - mavlink_safety_allowed_area_t packet; - packet.p1x = p1x; - packet.p1y = p1y; - packet.p1z = p1z; - packet.p2x = p2x; - packet.p2y = p2y; - packet.p2z = p2z; - packet.frame = frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)&packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)&packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_safety_allowed_area_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, p1x); - _mav_put_float(buf, 4, p1y); - _mav_put_float(buf, 8, p1z); - _mav_put_float(buf, 12, p2x); - _mav_put_float(buf, 16, p2y); - _mav_put_float(buf, 20, p2z); - _mav_put_uint8_t(buf, 24, frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); -#endif -#else - mavlink_safety_allowed_area_t *packet = (mavlink_safety_allowed_area_t *)msgbuf; - packet->p1x = p1x; - packet->p1y = p1y; - packet->p1z = p1z; - packet->p2x = p2x; - packet->p2y = p2y; - packet->p2z = p2z; - packet->frame = frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SAFETY_ALLOWED_AREA UNPACKING - - -/** - * @brief Get field frame from safety_allowed_area message - * - * @return Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - */ -static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 24); -} - -/** - * @brief Get field p1x from safety_allowed_area message - * - * @return x position 1 / Latitude 1 - */ -static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field p1y from safety_allowed_area message - * - * @return y position 1 / Longitude 1 - */ -static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field p1z from safety_allowed_area message - * - * @return z position 1 / Altitude 1 - */ -static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field p2x from safety_allowed_area message - * - * @return x position 2 / Latitude 2 - */ -static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field p2y from safety_allowed_area message - * - * @return y position 2 / Longitude 2 - */ -static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field p2z from safety_allowed_area message - * - * @return z position 2 / Altitude 2 - */ -static inline float mavlink_msg_safety_allowed_area_get_p2z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a safety_allowed_area message into a struct - * - * @param msg The message to decode - * @param safety_allowed_area C-struct to decode the message contents into - */ -static inline void mavlink_msg_safety_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_allowed_area_t* safety_allowed_area) -{ -#if MAVLINK_NEED_BYTE_SWAP - safety_allowed_area->p1x = mavlink_msg_safety_allowed_area_get_p1x(msg); - safety_allowed_area->p1y = mavlink_msg_safety_allowed_area_get_p1y(msg); - safety_allowed_area->p1z = mavlink_msg_safety_allowed_area_get_p1z(msg); - safety_allowed_area->p2x = mavlink_msg_safety_allowed_area_get_p2x(msg); - safety_allowed_area->p2y = mavlink_msg_safety_allowed_area_get_p2y(msg); - safety_allowed_area->p2z = mavlink_msg_safety_allowed_area_get_p2z(msg); - safety_allowed_area->frame = mavlink_msg_safety_allowed_area_get_frame(msg); -#else - memcpy(safety_allowed_area, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h deleted file mode 100644 index dafbc260b..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h +++ /dev/null @@ -1,401 +0,0 @@ -// MESSAGE SAFETY_SET_ALLOWED_AREA PACKING - -#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA 54 - -typedef struct __mavlink_safety_set_allowed_area_t -{ - float p1x; ///< x position 1 / Latitude 1 - float p1y; ///< y position 1 / Longitude 1 - float p1z; ///< z position 1 / Altitude 1 - float p2x; ///< x position 2 / Latitude 2 - float p2y; ///< y position 2 / Longitude 2 - float p2z; ///< z position 2 / Altitude 2 - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. -} mavlink_safety_set_allowed_area_t; - -#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN 27 -#define MAVLINK_MSG_ID_54_LEN 27 - -#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC 15 -#define MAVLINK_MSG_ID_54_CRC 15 - - - -#define MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA { \ - "SAFETY_SET_ALLOWED_AREA", \ - 9, \ - { { "p1x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_safety_set_allowed_area_t, p1x) }, \ - { "p1y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_safety_set_allowed_area_t, p1y) }, \ - { "p1z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_safety_set_allowed_area_t, p1z) }, \ - { "p2x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_safety_set_allowed_area_t, p2x) }, \ - { "p2y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_safety_set_allowed_area_t, p2y) }, \ - { "p2z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_safety_set_allowed_area_t, p2z) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_safety_set_allowed_area_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_safety_set_allowed_area_t, target_component) }, \ - { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_safety_set_allowed_area_t, frame) }, \ - } \ -} - - -/** - * @brief Pack a safety_set_allowed_area message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - * @param p1x x position 1 / Latitude 1 - * @param p1y y position 1 / Longitude 1 - * @param p1z z position 1 / Altitude 1 - * @param p2x x position 2 / Latitude 2 - * @param p2y y position 2 / Longitude 2 - * @param p2z z position 2 / Altitude 2 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN]; - _mav_put_float(buf, 0, p1x); - _mav_put_float(buf, 4, p1y); - _mav_put_float(buf, 8, p1z); - _mav_put_float(buf, 12, p2x); - _mav_put_float(buf, 16, p2y); - _mav_put_float(buf, 20, p2z); - _mav_put_uint8_t(buf, 24, target_system); - _mav_put_uint8_t(buf, 25, target_component); - _mav_put_uint8_t(buf, 26, frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); -#else - mavlink_safety_set_allowed_area_t packet; - packet.p1x = p1x; - packet.p1y = p1y; - packet.p1z = p1z; - packet.p2x = p2x; - packet.p2y = p2y; - packet.p2z = p2z; - packet.target_system = target_system; - packet.target_component = target_component; - packet.frame = frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); -#endif -} - -/** - * @brief Pack a safety_set_allowed_area message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - * @param p1x x position 1 / Latitude 1 - * @param p1y y position 1 / Longitude 1 - * @param p1z z position 1 / Altitude 1 - * @param p2x x position 2 / Latitude 2 - * @param p2y y position 2 / Longitude 2 - * @param p2z z position 2 / Altitude 2 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t frame,float p1x,float p1y,float p1z,float p2x,float p2y,float p2z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN]; - _mav_put_float(buf, 0, p1x); - _mav_put_float(buf, 4, p1y); - _mav_put_float(buf, 8, p1z); - _mav_put_float(buf, 12, p2x); - _mav_put_float(buf, 16, p2y); - _mav_put_float(buf, 20, p2z); - _mav_put_uint8_t(buf, 24, target_system); - _mav_put_uint8_t(buf, 25, target_component); - _mav_put_uint8_t(buf, 26, frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); -#else - mavlink_safety_set_allowed_area_t packet; - packet.p1x = p1x; - packet.p1y = p1y; - packet.p1z = p1z; - packet.p2x = p2x; - packet.p2y = p2y; - packet.p2z = p2z; - packet.target_system = target_system; - packet.target_component = target_component; - packet.frame = frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); -#endif -} - -/** - * @brief Encode a safety_set_allowed_area struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param safety_set_allowed_area C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_safety_set_allowed_area_t* safety_set_allowed_area) -{ - return mavlink_msg_safety_set_allowed_area_pack(system_id, component_id, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); -} - -/** - * @brief Encode a safety_set_allowed_area struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param safety_set_allowed_area C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_safety_set_allowed_area_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_safety_set_allowed_area_t* safety_set_allowed_area) -{ - return mavlink_msg_safety_set_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); -} - -/** - * @brief Send a safety_set_allowed_area message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - * @param p1x x position 1 / Latitude 1 - * @param p1y y position 1 / Longitude 1 - * @param p1z z position 1 / Altitude 1 - * @param p2x x position 2 / Latitude 2 - * @param p2y y position 2 / Longitude 2 - * @param p2z z position 2 / Altitude 2 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN]; - _mav_put_float(buf, 0, p1x); - _mav_put_float(buf, 4, p1y); - _mav_put_float(buf, 8, p1z); - _mav_put_float(buf, 12, p2x); - _mav_put_float(buf, 16, p2y); - _mav_put_float(buf, 20, p2z); - _mav_put_uint8_t(buf, 24, target_system); - _mav_put_uint8_t(buf, 25, target_component); - _mav_put_uint8_t(buf, 26, frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); -#endif -#else - mavlink_safety_set_allowed_area_t packet; - packet.p1x = p1x; - packet.p1y = p1y; - packet.p1z = p1z; - packet.p2x = p2x; - packet.p2y = p2y; - packet.p2z = p2z; - packet.target_system = target_system; - packet.target_component = target_component; - packet.frame = frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)&packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)&packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_safety_set_allowed_area_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, p1x); - _mav_put_float(buf, 4, p1y); - _mav_put_float(buf, 8, p1z); - _mav_put_float(buf, 12, p2x); - _mav_put_float(buf, 16, p2y); - _mav_put_float(buf, 20, p2z); - _mav_put_uint8_t(buf, 24, target_system); - _mav_put_uint8_t(buf, 25, target_component); - _mav_put_uint8_t(buf, 26, frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); -#endif -#else - mavlink_safety_set_allowed_area_t *packet = (mavlink_safety_set_allowed_area_t *)msgbuf; - packet->p1x = p1x; - packet->p1y = p1y; - packet->p1z = p1z; - packet->p2x = p2x; - packet->p2y = p2y; - packet->p2z = p2z; - packet->target_system = target_system; - packet->target_component = target_component; - packet->frame = frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SAFETY_SET_ALLOWED_AREA UNPACKING - - -/** - * @brief Get field target_system from safety_set_allowed_area message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 24); -} - -/** - * @brief Get field target_component from safety_set_allowed_area message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 25); -} - -/** - * @brief Get field frame from safety_set_allowed_area message - * - * @return Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - */ -static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 26); -} - -/** - * @brief Get field p1x from safety_set_allowed_area message - * - * @return x position 1 / Latitude 1 - */ -static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field p1y from safety_set_allowed_area message - * - * @return y position 1 / Longitude 1 - */ -static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field p1z from safety_set_allowed_area message - * - * @return z position 1 / Altitude 1 - */ -static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field p2x from safety_set_allowed_area message - * - * @return x position 2 / Latitude 2 - */ -static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field p2y from safety_set_allowed_area message - * - * @return y position 2 / Longitude 2 - */ -static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field p2z from safety_set_allowed_area message - * - * @return z position 2 / Altitude 2 - */ -static inline float mavlink_msg_safety_set_allowed_area_get_p2z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a safety_set_allowed_area message into a struct - * - * @param msg The message to decode - * @param safety_set_allowed_area C-struct to decode the message contents into - */ -static inline void mavlink_msg_safety_set_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_set_allowed_area_t* safety_set_allowed_area) -{ -#if MAVLINK_NEED_BYTE_SWAP - safety_set_allowed_area->p1x = mavlink_msg_safety_set_allowed_area_get_p1x(msg); - safety_set_allowed_area->p1y = mavlink_msg_safety_set_allowed_area_get_p1y(msg); - safety_set_allowed_area->p1z = mavlink_msg_safety_set_allowed_area_get_p1z(msg); - safety_set_allowed_area->p2x = mavlink_msg_safety_set_allowed_area_get_p2x(msg); - safety_set_allowed_area->p2y = mavlink_msg_safety_set_allowed_area_get_p2y(msg); - safety_set_allowed_area->p2z = mavlink_msg_safety_set_allowed_area_get_p2z(msg); - safety_set_allowed_area->target_system = mavlink_msg_safety_set_allowed_area_get_target_system(msg); - safety_set_allowed_area->target_component = mavlink_msg_safety_set_allowed_area_get_target_component(msg); - safety_set_allowed_area->frame = mavlink_msg_safety_set_allowed_area_get_frame(msg); -#else - memcpy(safety_set_allowed_area, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h deleted file mode 100644 index 1648a56f8..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h +++ /dev/null @@ -1,425 +0,0 @@ -// MESSAGE SCALED_IMU PACKING - -#define MAVLINK_MSG_ID_SCALED_IMU 26 - -typedef struct __mavlink_scaled_imu_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - int16_t xacc; ///< X acceleration (mg) - int16_t yacc; ///< Y acceleration (mg) - int16_t zacc; ///< Z acceleration (mg) - int16_t xgyro; ///< Angular speed around X axis (millirad /sec) - int16_t ygyro; ///< Angular speed around Y axis (millirad /sec) - int16_t zgyro; ///< Angular speed around Z axis (millirad /sec) - int16_t xmag; ///< X Magnetic field (milli tesla) - int16_t ymag; ///< Y Magnetic field (milli tesla) - int16_t zmag; ///< Z Magnetic field (milli tesla) -} mavlink_scaled_imu_t; - -#define MAVLINK_MSG_ID_SCALED_IMU_LEN 22 -#define MAVLINK_MSG_ID_26_LEN 22 - -#define MAVLINK_MSG_ID_SCALED_IMU_CRC 170 -#define MAVLINK_MSG_ID_26_CRC 170 - - - -#define MAVLINK_MESSAGE_INFO_SCALED_IMU { \ - "SCALED_IMU", \ - 10, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu_t, time_boot_ms) }, \ - { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu_t, xacc) }, \ - { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu_t, yacc) }, \ - { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_scaled_imu_t, zacc) }, \ - { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_scaled_imu_t, xgyro) }, \ - { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_imu_t, ygyro) }, \ - { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_imu_t, zgyro) }, \ - { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu_t, xmag) }, \ - { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu_t, ymag) }, \ - { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu_t, zmag) }, \ - } \ -} - - -/** - * @brief Pack a scaled_imu message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @param xgyro Angular speed around X axis (millirad /sec) - * @param ygyro Angular speed around Y axis (millirad /sec) - * @param zgyro Angular speed around Z axis (millirad /sec) - * @param xmag X Magnetic field (milli tesla) - * @param ymag Y Magnetic field (milli tesla) - * @param zmag Z Magnetic field (milli tesla) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, xacc); - _mav_put_int16_t(buf, 6, yacc); - _mav_put_int16_t(buf, 8, zacc); - _mav_put_int16_t(buf, 10, xgyro); - _mav_put_int16_t(buf, 12, ygyro); - _mav_put_int16_t(buf, 14, zgyro); - _mav_put_int16_t(buf, 16, xmag); - _mav_put_int16_t(buf, 18, ymag); - _mav_put_int16_t(buf, 20, zmag); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); -#else - mavlink_scaled_imu_t packet; - packet.time_boot_ms = time_boot_ms; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU_LEN); -#endif -} - -/** - * @brief Pack a scaled_imu message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @param xgyro Angular speed around X axis (millirad /sec) - * @param ygyro Angular speed around Y axis (millirad /sec) - * @param zgyro Angular speed around Z axis (millirad /sec) - * @param xmag X Magnetic field (milli tesla) - * @param ymag Y Magnetic field (milli tesla) - * @param zmag Z Magnetic field (milli tesla) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, xacc); - _mav_put_int16_t(buf, 6, yacc); - _mav_put_int16_t(buf, 8, zacc); - _mav_put_int16_t(buf, 10, xgyro); - _mav_put_int16_t(buf, 12, ygyro); - _mav_put_int16_t(buf, 14, zgyro); - _mav_put_int16_t(buf, 16, xmag); - _mav_put_int16_t(buf, 18, ymag); - _mav_put_int16_t(buf, 20, zmag); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); -#else - mavlink_scaled_imu_t packet; - packet.time_boot_ms = time_boot_ms; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU_LEN); -#endif -} - -/** - * @brief Encode a scaled_imu struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param scaled_imu C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) -{ - return mavlink_msg_scaled_imu_pack(system_id, component_id, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); -} - -/** - * @brief Encode a scaled_imu struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param scaled_imu C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_scaled_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) -{ - return mavlink_msg_scaled_imu_pack_chan(system_id, component_id, chan, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); -} - -/** - * @brief Send a scaled_imu message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @param xgyro Angular speed around X axis (millirad /sec) - * @param ygyro Angular speed around Y axis (millirad /sec) - * @param zgyro Angular speed around Z axis (millirad /sec) - * @param xmag X Magnetic field (milli tesla) - * @param ymag Y Magnetic field (milli tesla) - * @param zmag Z Magnetic field (milli tesla) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, xacc); - _mav_put_int16_t(buf, 6, yacc); - _mav_put_int16_t(buf, 8, zacc); - _mav_put_int16_t(buf, 10, xgyro); - _mav_put_int16_t(buf, 12, ygyro); - _mav_put_int16_t(buf, 14, zgyro); - _mav_put_int16_t(buf, 16, xmag); - _mav_put_int16_t(buf, 18, ymag); - _mav_put_int16_t(buf, 20, zmag); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); -#endif -#else - mavlink_scaled_imu_t packet; - packet.time_boot_ms = time_boot_ms; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SCALED_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, xacc); - _mav_put_int16_t(buf, 6, yacc); - _mav_put_int16_t(buf, 8, zacc); - _mav_put_int16_t(buf, 10, xgyro); - _mav_put_int16_t(buf, 12, ygyro); - _mav_put_int16_t(buf, 14, zgyro); - _mav_put_int16_t(buf, 16, xmag); - _mav_put_int16_t(buf, 18, ymag); - _mav_put_int16_t(buf, 20, zmag); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); -#endif -#else - mavlink_scaled_imu_t *packet = (mavlink_scaled_imu_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->xacc = xacc; - packet->yacc = yacc; - packet->zacc = zacc; - packet->xgyro = xgyro; - packet->ygyro = ygyro; - packet->zgyro = zgyro; - packet->xmag = xmag; - packet->ymag = ymag; - packet->zmag = zmag; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SCALED_IMU UNPACKING - - -/** - * @brief Get field time_boot_ms from scaled_imu message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_scaled_imu_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field xacc from scaled_imu message - * - * @return X acceleration (mg) - */ -static inline int16_t mavlink_msg_scaled_imu_get_xacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 4); -} - -/** - * @brief Get field yacc from scaled_imu message - * - * @return Y acceleration (mg) - */ -static inline int16_t mavlink_msg_scaled_imu_get_yacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 6); -} - -/** - * @brief Get field zacc from scaled_imu message - * - * @return Z acceleration (mg) - */ -static inline int16_t mavlink_msg_scaled_imu_get_zacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field xgyro from scaled_imu message - * - * @return Angular speed around X axis (millirad /sec) - */ -static inline int16_t mavlink_msg_scaled_imu_get_xgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 10); -} - -/** - * @brief Get field ygyro from scaled_imu message - * - * @return Angular speed around Y axis (millirad /sec) - */ -static inline int16_t mavlink_msg_scaled_imu_get_ygyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Get field zgyro from scaled_imu message - * - * @return Angular speed around Z axis (millirad /sec) - */ -static inline int16_t mavlink_msg_scaled_imu_get_zgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 14); -} - -/** - * @brief Get field xmag from scaled_imu message - * - * @return X Magnetic field (milli tesla) - */ -static inline int16_t mavlink_msg_scaled_imu_get_xmag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 16); -} - -/** - * @brief Get field ymag from scaled_imu message - * - * @return Y Magnetic field (milli tesla) - */ -static inline int16_t mavlink_msg_scaled_imu_get_ymag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 18); -} - -/** - * @brief Get field zmag from scaled_imu message - * - * @return Z Magnetic field (milli tesla) - */ -static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Decode a scaled_imu message into a struct - * - * @param msg The message to decode - * @param scaled_imu C-struct to decode the message contents into - */ -static inline void mavlink_msg_scaled_imu_decode(const mavlink_message_t* msg, mavlink_scaled_imu_t* scaled_imu) -{ -#if MAVLINK_NEED_BYTE_SWAP - scaled_imu->time_boot_ms = mavlink_msg_scaled_imu_get_time_boot_ms(msg); - scaled_imu->xacc = mavlink_msg_scaled_imu_get_xacc(msg); - scaled_imu->yacc = mavlink_msg_scaled_imu_get_yacc(msg); - scaled_imu->zacc = mavlink_msg_scaled_imu_get_zacc(msg); - scaled_imu->xgyro = mavlink_msg_scaled_imu_get_xgyro(msg); - scaled_imu->ygyro = mavlink_msg_scaled_imu_get_ygyro(msg); - scaled_imu->zgyro = mavlink_msg_scaled_imu_get_zgyro(msg); - scaled_imu->xmag = mavlink_msg_scaled_imu_get_xmag(msg); - scaled_imu->ymag = mavlink_msg_scaled_imu_get_ymag(msg); - scaled_imu->zmag = mavlink_msg_scaled_imu_get_zmag(msg); -#else - memcpy(scaled_imu, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_IMU_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h deleted file mode 100644 index 1dea121dd..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h +++ /dev/null @@ -1,425 +0,0 @@ -// MESSAGE SCALED_IMU2 PACKING - -#define MAVLINK_MSG_ID_SCALED_IMU2 116 - -typedef struct __mavlink_scaled_imu2_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - int16_t xacc; ///< X acceleration (mg) - int16_t yacc; ///< Y acceleration (mg) - int16_t zacc; ///< Z acceleration (mg) - int16_t xgyro; ///< Angular speed around X axis (millirad /sec) - int16_t ygyro; ///< Angular speed around Y axis (millirad /sec) - int16_t zgyro; ///< Angular speed around Z axis (millirad /sec) - int16_t xmag; ///< X Magnetic field (milli tesla) - int16_t ymag; ///< Y Magnetic field (milli tesla) - int16_t zmag; ///< Z Magnetic field (milli tesla) -} mavlink_scaled_imu2_t; - -#define MAVLINK_MSG_ID_SCALED_IMU2_LEN 22 -#define MAVLINK_MSG_ID_116_LEN 22 - -#define MAVLINK_MSG_ID_SCALED_IMU2_CRC 76 -#define MAVLINK_MSG_ID_116_CRC 76 - - - -#define MAVLINK_MESSAGE_INFO_SCALED_IMU2 { \ - "SCALED_IMU2", \ - 10, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu2_t, time_boot_ms) }, \ - { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu2_t, xacc) }, \ - { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu2_t, yacc) }, \ - { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_scaled_imu2_t, zacc) }, \ - { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_scaled_imu2_t, xgyro) }, \ - { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_imu2_t, ygyro) }, \ - { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_imu2_t, zgyro) }, \ - { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu2_t, xmag) }, \ - { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu2_t, ymag) }, \ - { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu2_t, zmag) }, \ - } \ -} - - -/** - * @brief Pack a scaled_imu2 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @param xgyro Angular speed around X axis (millirad /sec) - * @param ygyro Angular speed around Y axis (millirad /sec) - * @param zgyro Angular speed around Z axis (millirad /sec) - * @param xmag X Magnetic field (milli tesla) - * @param ymag Y Magnetic field (milli tesla) - * @param zmag Z Magnetic field (milli tesla) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, xacc); - _mav_put_int16_t(buf, 6, yacc); - _mav_put_int16_t(buf, 8, zacc); - _mav_put_int16_t(buf, 10, xgyro); - _mav_put_int16_t(buf, 12, ygyro); - _mav_put_int16_t(buf, 14, zgyro); - _mav_put_int16_t(buf, 16, xmag); - _mav_put_int16_t(buf, 18, ymag); - _mav_put_int16_t(buf, 20, zmag); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); -#else - mavlink_scaled_imu2_t packet; - packet.time_boot_ms = time_boot_ms; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SCALED_IMU2; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU2_LEN); -#endif -} - -/** - * @brief Pack a scaled_imu2 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @param xgyro Angular speed around X axis (millirad /sec) - * @param ygyro Angular speed around Y axis (millirad /sec) - * @param zgyro Angular speed around Z axis (millirad /sec) - * @param xmag X Magnetic field (milli tesla) - * @param ymag Y Magnetic field (milli tesla) - * @param zmag Z Magnetic field (milli tesla) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, xacc); - _mav_put_int16_t(buf, 6, yacc); - _mav_put_int16_t(buf, 8, zacc); - _mav_put_int16_t(buf, 10, xgyro); - _mav_put_int16_t(buf, 12, ygyro); - _mav_put_int16_t(buf, 14, zgyro); - _mav_put_int16_t(buf, 16, xmag); - _mav_put_int16_t(buf, 18, ymag); - _mav_put_int16_t(buf, 20, zmag); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); -#else - mavlink_scaled_imu2_t packet; - packet.time_boot_ms = time_boot_ms; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SCALED_IMU2; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU2_LEN); -#endif -} - -/** - * @brief Encode a scaled_imu2 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param scaled_imu2 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_scaled_imu2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2) -{ - return mavlink_msg_scaled_imu2_pack(system_id, component_id, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag); -} - -/** - * @brief Encode a scaled_imu2 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param scaled_imu2 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_scaled_imu2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2) -{ - return mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, chan, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag); -} - -/** - * @brief Send a scaled_imu2 message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @param xgyro Angular speed around X axis (millirad /sec) - * @param ygyro Angular speed around Y axis (millirad /sec) - * @param zgyro Angular speed around Z axis (millirad /sec) - * @param xmag X Magnetic field (milli tesla) - * @param ymag Y Magnetic field (milli tesla) - * @param zmag Z Magnetic field (milli tesla) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, xacc); - _mav_put_int16_t(buf, 6, yacc); - _mav_put_int16_t(buf, 8, zacc); - _mav_put_int16_t(buf, 10, xgyro); - _mav_put_int16_t(buf, 12, ygyro); - _mav_put_int16_t(buf, 14, zgyro); - _mav_put_int16_t(buf, 16, xmag); - _mav_put_int16_t(buf, 18, ymag); - _mav_put_int16_t(buf, 20, zmag); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); -#endif -#else - mavlink_scaled_imu2_t packet; - packet.time_boot_ms = time_boot_ms; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SCALED_IMU2_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, xacc); - _mav_put_int16_t(buf, 6, yacc); - _mav_put_int16_t(buf, 8, zacc); - _mav_put_int16_t(buf, 10, xgyro); - _mav_put_int16_t(buf, 12, ygyro); - _mav_put_int16_t(buf, 14, zgyro); - _mav_put_int16_t(buf, 16, xmag); - _mav_put_int16_t(buf, 18, ymag); - _mav_put_int16_t(buf, 20, zmag); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); -#endif -#else - mavlink_scaled_imu2_t *packet = (mavlink_scaled_imu2_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->xacc = xacc; - packet->yacc = yacc; - packet->zacc = zacc; - packet->xgyro = xgyro; - packet->ygyro = ygyro; - packet->zgyro = zgyro; - packet->xmag = xmag; - packet->ymag = ymag; - packet->zmag = zmag; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SCALED_IMU2 UNPACKING - - -/** - * @brief Get field time_boot_ms from scaled_imu2 message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_scaled_imu2_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field xacc from scaled_imu2 message - * - * @return X acceleration (mg) - */ -static inline int16_t mavlink_msg_scaled_imu2_get_xacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 4); -} - -/** - * @brief Get field yacc from scaled_imu2 message - * - * @return Y acceleration (mg) - */ -static inline int16_t mavlink_msg_scaled_imu2_get_yacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 6); -} - -/** - * @brief Get field zacc from scaled_imu2 message - * - * @return Z acceleration (mg) - */ -static inline int16_t mavlink_msg_scaled_imu2_get_zacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field xgyro from scaled_imu2 message - * - * @return Angular speed around X axis (millirad /sec) - */ -static inline int16_t mavlink_msg_scaled_imu2_get_xgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 10); -} - -/** - * @brief Get field ygyro from scaled_imu2 message - * - * @return Angular speed around Y axis (millirad /sec) - */ -static inline int16_t mavlink_msg_scaled_imu2_get_ygyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Get field zgyro from scaled_imu2 message - * - * @return Angular speed around Z axis (millirad /sec) - */ -static inline int16_t mavlink_msg_scaled_imu2_get_zgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 14); -} - -/** - * @brief Get field xmag from scaled_imu2 message - * - * @return X Magnetic field (milli tesla) - */ -static inline int16_t mavlink_msg_scaled_imu2_get_xmag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 16); -} - -/** - * @brief Get field ymag from scaled_imu2 message - * - * @return Y Magnetic field (milli tesla) - */ -static inline int16_t mavlink_msg_scaled_imu2_get_ymag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 18); -} - -/** - * @brief Get field zmag from scaled_imu2 message - * - * @return Z Magnetic field (milli tesla) - */ -static inline int16_t mavlink_msg_scaled_imu2_get_zmag(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Decode a scaled_imu2 message into a struct - * - * @param msg The message to decode - * @param scaled_imu2 C-struct to decode the message contents into - */ -static inline void mavlink_msg_scaled_imu2_decode(const mavlink_message_t* msg, mavlink_scaled_imu2_t* scaled_imu2) -{ -#if MAVLINK_NEED_BYTE_SWAP - scaled_imu2->time_boot_ms = mavlink_msg_scaled_imu2_get_time_boot_ms(msg); - scaled_imu2->xacc = mavlink_msg_scaled_imu2_get_xacc(msg); - scaled_imu2->yacc = mavlink_msg_scaled_imu2_get_yacc(msg); - scaled_imu2->zacc = mavlink_msg_scaled_imu2_get_zacc(msg); - scaled_imu2->xgyro = mavlink_msg_scaled_imu2_get_xgyro(msg); - scaled_imu2->ygyro = mavlink_msg_scaled_imu2_get_ygyro(msg); - scaled_imu2->zgyro = mavlink_msg_scaled_imu2_get_zgyro(msg); - scaled_imu2->xmag = mavlink_msg_scaled_imu2_get_xmag(msg); - scaled_imu2->ymag = mavlink_msg_scaled_imu2_get_ymag(msg); - scaled_imu2->zmag = mavlink_msg_scaled_imu2_get_zmag(msg); -#else - memcpy(scaled_imu2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_IMU2_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h deleted file mode 100644 index e0d6d8766..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE SCALED_PRESSURE PACKING - -#define MAVLINK_MSG_ID_SCALED_PRESSURE 29 - -typedef struct __mavlink_scaled_pressure_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - float press_abs; ///< Absolute pressure (hectopascal) - float press_diff; ///< Differential pressure 1 (hectopascal) - int16_t temperature; ///< Temperature measurement (0.01 degrees celsius) -} mavlink_scaled_pressure_t; - -#define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 14 -#define MAVLINK_MSG_ID_29_LEN 14 - -#define MAVLINK_MSG_ID_SCALED_PRESSURE_CRC 115 -#define MAVLINK_MSG_ID_29_CRC 115 - - - -#define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE { \ - "SCALED_PRESSURE", \ - 4, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure_t, time_boot_ms) }, \ - { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure_t, press_abs) }, \ - { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure_t, press_diff) }, \ - { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure_t, temperature) }, \ - } \ -} - - -/** - * @brief Pack a scaled_pressure message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param press_abs Absolute pressure (hectopascal) - * @param press_diff Differential pressure 1 (hectopascal) - * @param temperature Temperature measurement (0.01 degrees celsius) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, press_abs); - _mav_put_float(buf, 8, press_diff); - _mav_put_int16_t(buf, 12, temperature); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); -#else - mavlink_scaled_pressure_t packet; - packet.time_boot_ms = time_boot_ms; - packet.press_abs = press_abs; - packet.press_diff = press_diff; - packet.temperature = temperature; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); -#endif -} - -/** - * @brief Pack a scaled_pressure message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param press_abs Absolute pressure (hectopascal) - * @param press_diff Differential pressure 1 (hectopascal) - * @param temperature Temperature measurement (0.01 degrees celsius) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, press_abs); - _mav_put_float(buf, 8, press_diff); - _mav_put_int16_t(buf, 12, temperature); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); -#else - mavlink_scaled_pressure_t packet; - packet.time_boot_ms = time_boot_ms; - packet.press_abs = press_abs; - packet.press_diff = press_diff; - packet.temperature = temperature; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); -#endif -} - -/** - * @brief Encode a scaled_pressure struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param scaled_pressure C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) -{ - return mavlink_msg_scaled_pressure_pack(system_id, component_id, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); -} - -/** - * @brief Encode a scaled_pressure struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param scaled_pressure C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_scaled_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) -{ - return mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, chan, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); -} - -/** - * @brief Send a scaled_pressure message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param press_abs Absolute pressure (hectopascal) - * @param press_diff Differential pressure 1 (hectopascal) - * @param temperature Temperature measurement (0.01 degrees celsius) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, press_abs); - _mav_put_float(buf, 8, press_diff); - _mav_put_int16_t(buf, 12, temperature); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); -#endif -#else - mavlink_scaled_pressure_t packet; - packet.time_boot_ms = time_boot_ms; - packet.press_abs = press_abs; - packet.press_diff = press_diff; - packet.temperature = temperature; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SCALED_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, press_abs); - _mav_put_float(buf, 8, press_diff); - _mav_put_int16_t(buf, 12, temperature); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); -#endif -#else - mavlink_scaled_pressure_t *packet = (mavlink_scaled_pressure_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->press_abs = press_abs; - packet->press_diff = press_diff; - packet->temperature = temperature; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SCALED_PRESSURE UNPACKING - - -/** - * @brief Get field time_boot_ms from scaled_pressure message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_scaled_pressure_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field press_abs from scaled_pressure message - * - * @return Absolute pressure (hectopascal) - */ -static inline float mavlink_msg_scaled_pressure_get_press_abs(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field press_diff from scaled_pressure message - * - * @return Differential pressure 1 (hectopascal) - */ -static inline float mavlink_msg_scaled_pressure_get_press_diff(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field temperature from scaled_pressure message - * - * @return Temperature measurement (0.01 degrees celsius) - */ -static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Decode a scaled_pressure message into a struct - * - * @param msg The message to decode - * @param scaled_pressure C-struct to decode the message contents into - */ -static inline void mavlink_msg_scaled_pressure_decode(const mavlink_message_t* msg, mavlink_scaled_pressure_t* scaled_pressure) -{ -#if MAVLINK_NEED_BYTE_SWAP - scaled_pressure->time_boot_ms = mavlink_msg_scaled_pressure_get_time_boot_ms(msg); - scaled_pressure->press_abs = mavlink_msg_scaled_pressure_get_press_abs(msg); - scaled_pressure->press_diff = mavlink_msg_scaled_pressure_get_press_diff(msg); - scaled_pressure->temperature = mavlink_msg_scaled_pressure_get_temperature(msg); -#else - memcpy(scaled_pressure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h deleted file mode 100644 index cbf72bae3..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h +++ /dev/null @@ -1,321 +0,0 @@ -// MESSAGE SERIAL_CONTROL PACKING - -#define MAVLINK_MSG_ID_SERIAL_CONTROL 126 - -typedef struct __mavlink_serial_control_t -{ - uint32_t baudrate; ///< Baudrate of transfer. Zero means no change. - uint16_t timeout; ///< Timeout for reply data in milliseconds - uint8_t device; ///< See SERIAL_CONTROL_DEV enum - uint8_t flags; ///< See SERIAL_CONTROL_FLAG enum - uint8_t count; ///< how many bytes in this transfer - uint8_t data[70]; ///< serial data -} mavlink_serial_control_t; - -#define MAVLINK_MSG_ID_SERIAL_CONTROL_LEN 79 -#define MAVLINK_MSG_ID_126_LEN 79 - -#define MAVLINK_MSG_ID_SERIAL_CONTROL_CRC 220 -#define MAVLINK_MSG_ID_126_CRC 220 - -#define MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN 70 - -#define MAVLINK_MESSAGE_INFO_SERIAL_CONTROL { \ - "SERIAL_CONTROL", \ - 6, \ - { { "baudrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_control_t, baudrate) }, \ - { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_serial_control_t, timeout) }, \ - { "device", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_serial_control_t, device) }, \ - { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_serial_control_t, flags) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_serial_control_t, count) }, \ - { "data", NULL, MAVLINK_TYPE_UINT8_T, 70, 9, offsetof(mavlink_serial_control_t, data) }, \ - } \ -} - - -/** - * @brief Pack a serial_control message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param device See SERIAL_CONTROL_DEV enum - * @param flags See SERIAL_CONTROL_FLAG enum - * @param timeout Timeout for reply data in milliseconds - * @param baudrate Baudrate of transfer. Zero means no change. - * @param count how many bytes in this transfer - * @param data serial data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; - _mav_put_uint32_t(buf, 0, baudrate); - _mav_put_uint16_t(buf, 4, timeout); - _mav_put_uint8_t(buf, 6, device); - _mav_put_uint8_t(buf, 7, flags); - _mav_put_uint8_t(buf, 8, count); - _mav_put_uint8_t_array(buf, 9, data, 70); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); -#else - mavlink_serial_control_t packet; - packet.baudrate = baudrate; - packet.timeout = timeout; - packet.device = device; - packet.flags = flags; - packet.count = count; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); -#endif -} - -/** - * @brief Pack a serial_control message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param device See SERIAL_CONTROL_DEV enum - * @param flags See SERIAL_CONTROL_FLAG enum - * @param timeout Timeout for reply data in milliseconds - * @param baudrate Baudrate of transfer. Zero means no change. - * @param count how many bytes in this transfer - * @param data serial data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t device,uint8_t flags,uint16_t timeout,uint32_t baudrate,uint8_t count,const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; - _mav_put_uint32_t(buf, 0, baudrate); - _mav_put_uint16_t(buf, 4, timeout); - _mav_put_uint8_t(buf, 6, device); - _mav_put_uint8_t(buf, 7, flags); - _mav_put_uint8_t(buf, 8, count); - _mav_put_uint8_t_array(buf, 9, data, 70); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); -#else - mavlink_serial_control_t packet; - packet.baudrate = baudrate; - packet.timeout = timeout; - packet.device = device; - packet.flags = flags; - packet.count = count; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); -#endif -} - -/** - * @brief Encode a serial_control struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) -{ - return mavlink_msg_serial_control_pack(system_id, component_id, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); -} - -/** - * @brief Encode a serial_control struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) -{ - return mavlink_msg_serial_control_pack_chan(system_id, component_id, chan, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); -} - -/** - * @brief Send a serial_control message - * @param chan MAVLink channel to send the message - * - * @param device See SERIAL_CONTROL_DEV enum - * @param flags See SERIAL_CONTROL_FLAG enum - * @param timeout Timeout for reply data in milliseconds - * @param baudrate Baudrate of transfer. Zero means no change. - * @param count how many bytes in this transfer - * @param data serial data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_control_send(mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; - _mav_put_uint32_t(buf, 0, baudrate); - _mav_put_uint16_t(buf, 4, timeout); - _mav_put_uint8_t(buf, 6, device); - _mav_put_uint8_t(buf, 7, flags); - _mav_put_uint8_t(buf, 8, count); - _mav_put_uint8_t_array(buf, 9, data, 70); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); -#endif -#else - mavlink_serial_control_t packet; - packet.baudrate = baudrate; - packet.timeout = timeout; - packet.device = device; - packet.flags = flags; - packet.count = count; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERIAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_serial_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, baudrate); - _mav_put_uint16_t(buf, 4, timeout); - _mav_put_uint8_t(buf, 6, device); - _mav_put_uint8_t(buf, 7, flags); - _mav_put_uint8_t(buf, 8, count); - _mav_put_uint8_t_array(buf, 9, data, 70); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); -#endif -#else - mavlink_serial_control_t *packet = (mavlink_serial_control_t *)msgbuf; - packet->baudrate = baudrate; - packet->timeout = timeout; - packet->device = device; - packet->flags = flags; - packet->count = count; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*70); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERIAL_CONTROL UNPACKING - - -/** - * @brief Get field device from serial_control message - * - * @return See SERIAL_CONTROL_DEV enum - */ -static inline uint8_t mavlink_msg_serial_control_get_device(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field flags from serial_control message - * - * @return See SERIAL_CONTROL_FLAG enum - */ -static inline uint8_t mavlink_msg_serial_control_get_flags(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field timeout from serial_control message - * - * @return Timeout for reply data in milliseconds - */ -static inline uint16_t mavlink_msg_serial_control_get_timeout(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field baudrate from serial_control message - * - * @return Baudrate of transfer. Zero means no change. - */ -static inline uint32_t mavlink_msg_serial_control_get_baudrate(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field count from serial_control message - * - * @return how many bytes in this transfer - */ -static inline uint8_t mavlink_msg_serial_control_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field data from serial_control message - * - * @return serial data - */ -static inline uint16_t mavlink_msg_serial_control_get_data(const mavlink_message_t* msg, uint8_t *data) -{ - return _MAV_RETURN_uint8_t_array(msg, data, 70, 9); -} - -/** - * @brief Decode a serial_control message into a struct - * - * @param msg The message to decode - * @param serial_control C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_control_decode(const mavlink_message_t* msg, mavlink_serial_control_t* serial_control) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_control->baudrate = mavlink_msg_serial_control_get_baudrate(msg); - serial_control->timeout = mavlink_msg_serial_control_get_timeout(msg); - serial_control->device = mavlink_msg_serial_control_get_device(msg); - serial_control->flags = mavlink_msg_serial_control_get_flags(msg); - serial_control->count = mavlink_msg_serial_control_get_count(msg); - mavlink_msg_serial_control_get_data(msg, serial_control->data); -#else - memcpy(serial_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h deleted file mode 100644 index e8408862f..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h +++ /dev/null @@ -1,425 +0,0 @@ -// MESSAGE SERVO_OUTPUT_RAW PACKING - -#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 36 - -typedef struct __mavlink_servo_output_raw_t -{ - uint32_t time_usec; ///< Timestamp (microseconds since system boot) - uint16_t servo1_raw; ///< Servo output 1 value, in microseconds - uint16_t servo2_raw; ///< Servo output 2 value, in microseconds - uint16_t servo3_raw; ///< Servo output 3 value, in microseconds - uint16_t servo4_raw; ///< Servo output 4 value, in microseconds - uint16_t servo5_raw; ///< Servo output 5 value, in microseconds - uint16_t servo6_raw; ///< Servo output 6 value, in microseconds - uint16_t servo7_raw; ///< Servo output 7 value, in microseconds - uint16_t servo8_raw; ///< Servo output 8 value, in microseconds - uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. -} mavlink_servo_output_raw_t; - -#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 21 -#define MAVLINK_MSG_ID_36_LEN 21 - -#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC 222 -#define MAVLINK_MSG_ID_36_CRC 222 - - - -#define MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW { \ - "SERVO_OUTPUT_RAW", \ - 10, \ - { { "time_usec", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_servo_output_raw_t, time_usec) }, \ - { "servo1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_servo_output_raw_t, servo1_raw) }, \ - { "servo2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_servo_output_raw_t, servo2_raw) }, \ - { "servo3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_servo_output_raw_t, servo3_raw) }, \ - { "servo4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_servo_output_raw_t, servo4_raw) }, \ - { "servo5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_servo_output_raw_t, servo5_raw) }, \ - { "servo6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_servo_output_raw_t, servo6_raw) }, \ - { "servo7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_servo_output_raw_t, servo7_raw) }, \ - { "servo8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_servo_output_raw_t, servo8_raw) }, \ - { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_servo_output_raw_t, port) }, \ - } \ -} - - -/** - * @brief Pack a servo_output_raw message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_usec Timestamp (microseconds since system boot) - * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - * @param servo1_raw Servo output 1 value, in microseconds - * @param servo2_raw Servo output 2 value, in microseconds - * @param servo3_raw Servo output 3 value, in microseconds - * @param servo4_raw Servo output 4 value, in microseconds - * @param servo5_raw Servo output 5 value, in microseconds - * @param servo6_raw Servo output 6 value, in microseconds - * @param servo7_raw Servo output 7 value, in microseconds - * @param servo8_raw Servo output 8 value, in microseconds - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; - _mav_put_uint32_t(buf, 0, time_usec); - _mav_put_uint16_t(buf, 4, servo1_raw); - _mav_put_uint16_t(buf, 6, servo2_raw); - _mav_put_uint16_t(buf, 8, servo3_raw); - _mav_put_uint16_t(buf, 10, servo4_raw); - _mav_put_uint16_t(buf, 12, servo5_raw); - _mav_put_uint16_t(buf, 14, servo6_raw); - _mav_put_uint16_t(buf, 16, servo7_raw); - _mav_put_uint16_t(buf, 18, servo8_raw); - _mav_put_uint8_t(buf, 20, port); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); -#else - mavlink_servo_output_raw_t packet; - packet.time_usec = time_usec; - packet.servo1_raw = servo1_raw; - packet.servo2_raw = servo2_raw; - packet.servo3_raw = servo3_raw; - packet.servo4_raw = servo4_raw; - packet.servo5_raw = servo5_raw; - packet.servo6_raw = servo6_raw; - packet.servo7_raw = servo7_raw; - packet.servo8_raw = servo8_raw; - packet.port = port; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); -#endif -} - -/** - * @brief Pack a servo_output_raw message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp (microseconds since system boot) - * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - * @param servo1_raw Servo output 1 value, in microseconds - * @param servo2_raw Servo output 2 value, in microseconds - * @param servo3_raw Servo output 3 value, in microseconds - * @param servo4_raw Servo output 4 value, in microseconds - * @param servo5_raw Servo output 5 value, in microseconds - * @param servo6_raw Servo output 6 value, in microseconds - * @param servo7_raw Servo output 7 value, in microseconds - * @param servo8_raw Servo output 8 value, in microseconds - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_usec,uint8_t port,uint16_t servo1_raw,uint16_t servo2_raw,uint16_t servo3_raw,uint16_t servo4_raw,uint16_t servo5_raw,uint16_t servo6_raw,uint16_t servo7_raw,uint16_t servo8_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; - _mav_put_uint32_t(buf, 0, time_usec); - _mav_put_uint16_t(buf, 4, servo1_raw); - _mav_put_uint16_t(buf, 6, servo2_raw); - _mav_put_uint16_t(buf, 8, servo3_raw); - _mav_put_uint16_t(buf, 10, servo4_raw); - _mav_put_uint16_t(buf, 12, servo5_raw); - _mav_put_uint16_t(buf, 14, servo6_raw); - _mav_put_uint16_t(buf, 16, servo7_raw); - _mav_put_uint16_t(buf, 18, servo8_raw); - _mav_put_uint8_t(buf, 20, port); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); -#else - mavlink_servo_output_raw_t packet; - packet.time_usec = time_usec; - packet.servo1_raw = servo1_raw; - packet.servo2_raw = servo2_raw; - packet.servo3_raw = servo3_raw; - packet.servo4_raw = servo4_raw; - packet.servo5_raw = servo5_raw; - packet.servo6_raw = servo6_raw; - packet.servo7_raw = servo7_raw; - packet.servo8_raw = servo8_raw; - packet.port = port; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); -#endif -} - -/** - * @brief Encode a servo_output_raw struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param servo_output_raw C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) -{ - return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); -} - -/** - * @brief Encode a servo_output_raw struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param servo_output_raw C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_servo_output_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) -{ - return mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, chan, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); -} - -/** - * @brief Send a servo_output_raw message - * @param chan MAVLink channel to send the message - * - * @param time_usec Timestamp (microseconds since system boot) - * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - * @param servo1_raw Servo output 1 value, in microseconds - * @param servo2_raw Servo output 2 value, in microseconds - * @param servo3_raw Servo output 3 value, in microseconds - * @param servo4_raw Servo output 4 value, in microseconds - * @param servo5_raw Servo output 5 value, in microseconds - * @param servo6_raw Servo output 6 value, in microseconds - * @param servo7_raw Servo output 7 value, in microseconds - * @param servo8_raw Servo output 8 value, in microseconds - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; - _mav_put_uint32_t(buf, 0, time_usec); - _mav_put_uint16_t(buf, 4, servo1_raw); - _mav_put_uint16_t(buf, 6, servo2_raw); - _mav_put_uint16_t(buf, 8, servo3_raw); - _mav_put_uint16_t(buf, 10, servo4_raw); - _mav_put_uint16_t(buf, 12, servo5_raw); - _mav_put_uint16_t(buf, 14, servo6_raw); - _mav_put_uint16_t(buf, 16, servo7_raw); - _mav_put_uint16_t(buf, 18, servo8_raw); - _mav_put_uint8_t(buf, 20, port); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); -#endif -#else - mavlink_servo_output_raw_t packet; - packet.time_usec = time_usec; - packet.servo1_raw = servo1_raw; - packet.servo2_raw = servo2_raw; - packet.servo3_raw = servo3_raw; - packet.servo4_raw = servo4_raw; - packet.servo5_raw = servo5_raw; - packet.servo6_raw = servo6_raw; - packet.servo7_raw = servo7_raw; - packet.servo8_raw = servo8_raw; - packet.port = port; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)&packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)&packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_usec); - _mav_put_uint16_t(buf, 4, servo1_raw); - _mav_put_uint16_t(buf, 6, servo2_raw); - _mav_put_uint16_t(buf, 8, servo3_raw); - _mav_put_uint16_t(buf, 10, servo4_raw); - _mav_put_uint16_t(buf, 12, servo5_raw); - _mav_put_uint16_t(buf, 14, servo6_raw); - _mav_put_uint16_t(buf, 16, servo7_raw); - _mav_put_uint16_t(buf, 18, servo8_raw); - _mav_put_uint8_t(buf, 20, port); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); -#endif -#else - mavlink_servo_output_raw_t *packet = (mavlink_servo_output_raw_t *)msgbuf; - packet->time_usec = time_usec; - packet->servo1_raw = servo1_raw; - packet->servo2_raw = servo2_raw; - packet->servo3_raw = servo3_raw; - packet->servo4_raw = servo4_raw; - packet->servo5_raw = servo5_raw; - packet->servo6_raw = servo6_raw; - packet->servo7_raw = servo7_raw; - packet->servo8_raw = servo8_raw; - packet->port = port; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERVO_OUTPUT_RAW UNPACKING - - -/** - * @brief Get field time_usec from servo_output_raw message - * - * @return Timestamp (microseconds since system boot) - */ -static inline uint32_t mavlink_msg_servo_output_raw_get_time_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field port from servo_output_raw message - * - * @return Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - */ -static inline uint8_t mavlink_msg_servo_output_raw_get_port(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 20); -} - -/** - * @brief Get field servo1_raw from servo_output_raw message - * - * @return Servo output 1 value, in microseconds - */ -static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field servo2_raw from servo_output_raw message - * - * @return Servo output 2 value, in microseconds - */ -static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field servo3_raw from servo_output_raw message - * - * @return Servo output 3 value, in microseconds - */ -static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field servo4_raw from servo_output_raw message - * - * @return Servo output 4 value, in microseconds - */ -static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Get field servo5_raw from servo_output_raw message - * - * @return Servo output 5 value, in microseconds - */ -static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field servo6_raw from servo_output_raw message - * - * @return Servo output 6 value, in microseconds - */ -static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 14); -} - -/** - * @brief Get field servo7_raw from servo_output_raw message - * - * @return Servo output 7 value, in microseconds - */ -static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field servo8_raw from servo_output_raw message - * - * @return Servo output 8 value, in microseconds - */ -static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 18); -} - -/** - * @brief Decode a servo_output_raw message into a struct - * - * @param msg The message to decode - * @param servo_output_raw C-struct to decode the message contents into - */ -static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t* msg, mavlink_servo_output_raw_t* servo_output_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP - servo_output_raw->time_usec = mavlink_msg_servo_output_raw_get_time_usec(msg); - servo_output_raw->servo1_raw = mavlink_msg_servo_output_raw_get_servo1_raw(msg); - servo_output_raw->servo2_raw = mavlink_msg_servo_output_raw_get_servo2_raw(msg); - servo_output_raw->servo3_raw = mavlink_msg_servo_output_raw_get_servo3_raw(msg); - servo_output_raw->servo4_raw = mavlink_msg_servo_output_raw_get_servo4_raw(msg); - servo_output_raw->servo5_raw = mavlink_msg_servo_output_raw_get_servo5_raw(msg); - servo_output_raw->servo6_raw = mavlink_msg_servo_output_raw_get_servo6_raw(msg); - servo_output_raw->servo7_raw = mavlink_msg_servo_output_raw_get_servo7_raw(msg); - servo_output_raw->servo8_raw = mavlink_msg_servo_output_raw_get_servo8_raw(msg); - servo_output_raw->port = mavlink_msg_servo_output_raw_get_port(msg); -#else - memcpy(servo_output_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h deleted file mode 100644 index 94a7b432b..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE SET_GLOBAL_POSITION_SETPOINT_INT PACKING - -#define MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT 53 - -typedef struct __mavlink_set_global_position_setpoint_int_t -{ - int32_t latitude; ///< Latitude (WGS84), in degrees * 1E7 - int32_t longitude; ///< Longitude (WGS84), in degrees * 1E7 - int32_t altitude; ///< Altitude (WGS84), in meters * 1000 (positive for up) - int16_t yaw; ///< Desired yaw angle in degrees * 100 - uint8_t coordinate_frame; ///< Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT -} mavlink_set_global_position_setpoint_int_t; - -#define MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN 15 -#define MAVLINK_MSG_ID_53_LEN 15 - -#define MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC 33 -#define MAVLINK_MSG_ID_53_CRC 33 - - - -#define MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT { \ - "SET_GLOBAL_POSITION_SETPOINT_INT", \ - 5, \ - { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_global_position_setpoint_int_t, latitude) }, \ - { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_global_position_setpoint_int_t, longitude) }, \ - { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_set_global_position_setpoint_int_t, altitude) }, \ - { "yaw", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_set_global_position_setpoint_int_t, yaw) }, \ - { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_set_global_position_setpoint_int_t, coordinate_frame) }, \ - } \ -} - - -/** - * @brief Pack a set_global_position_setpoint_int message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#else - mavlink_set_global_position_setpoint_int_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -} - -/** - * @brief Pack a set_global_position_setpoint_int message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t coordinate_frame,int32_t latitude,int32_t longitude,int32_t altitude,int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#else - mavlink_set_global_position_setpoint_int_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -} - -/** - * @brief Encode a set_global_position_setpoint_int struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_global_position_setpoint_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_global_position_setpoint_int_t* set_global_position_setpoint_int) -{ - return mavlink_msg_set_global_position_setpoint_int_pack(system_id, component_id, msg, set_global_position_setpoint_int->coordinate_frame, set_global_position_setpoint_int->latitude, set_global_position_setpoint_int->longitude, set_global_position_setpoint_int->altitude, set_global_position_setpoint_int->yaw); -} - -/** - * @brief Encode a set_global_position_setpoint_int struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_global_position_setpoint_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_global_position_setpoint_int_t* set_global_position_setpoint_int) -{ - return mavlink_msg_set_global_position_setpoint_int_pack_chan(system_id, component_id, chan, msg, set_global_position_setpoint_int->coordinate_frame, set_global_position_setpoint_int->latitude, set_global_position_setpoint_int->longitude, set_global_position_setpoint_int->altitude, set_global_position_setpoint_int->yaw); -} - -/** - * @brief Send a set_global_position_setpoint_int message - * @param chan MAVLink channel to send the message - * - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84), in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_global_position_setpoint_int_send(mavlink_channel_t chan, uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -#else - mavlink_set_global_position_setpoint_int_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.yaw = yaw; - packet.coordinate_frame = coordinate_frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)&packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)&packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_set_global_position_setpoint_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_int16_t(buf, 12, yaw); - _mav_put_uint8_t(buf, 14, coordinate_frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -#else - mavlink_set_global_position_setpoint_int_t *packet = (mavlink_set_global_position_setpoint_int_t *)msgbuf; - packet->latitude = latitude; - packet->longitude = longitude; - packet->altitude = altitude; - packet->yaw = yaw; - packet->coordinate_frame = coordinate_frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SET_GLOBAL_POSITION_SETPOINT_INT UNPACKING - - -/** - * @brief Get field coordinate_frame from set_global_position_setpoint_int message - * - * @return Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT - */ -static inline uint8_t mavlink_msg_set_global_position_setpoint_int_get_coordinate_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field latitude from set_global_position_setpoint_int message - * - * @return Latitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_latitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field longitude from set_global_position_setpoint_int message - * - * @return Longitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_longitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field altitude from set_global_position_setpoint_int message - * - * @return Altitude (WGS84), in meters * 1000 (positive for up) - */ -static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field yaw from set_global_position_setpoint_int message - * - * @return Desired yaw angle in degrees * 100 - */ -static inline int16_t mavlink_msg_set_global_position_setpoint_int_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Decode a set_global_position_setpoint_int message into a struct - * - * @param msg The message to decode - * @param set_global_position_setpoint_int C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_global_position_setpoint_int_decode(const mavlink_message_t* msg, mavlink_set_global_position_setpoint_int_t* set_global_position_setpoint_int) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_global_position_setpoint_int->latitude = mavlink_msg_set_global_position_setpoint_int_get_latitude(msg); - set_global_position_setpoint_int->longitude = mavlink_msg_set_global_position_setpoint_int_get_longitude(msg); - set_global_position_setpoint_int->altitude = mavlink_msg_set_global_position_setpoint_int_get_altitude(msg); - set_global_position_setpoint_int->yaw = mavlink_msg_set_global_position_setpoint_int_get_yaw(msg); - set_global_position_setpoint_int->coordinate_frame = mavlink_msg_set_global_position_setpoint_int_get_coordinate_frame(msg); -#else - memcpy(set_global_position_setpoint_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h deleted file mode 100644 index 1c018cbee..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE SET_GPS_GLOBAL_ORIGIN PACKING - -#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN 48 - -typedef struct __mavlink_set_gps_global_origin_t -{ - int32_t latitude; ///< Latitude (WGS84), in degrees * 1E7 - int32_t longitude; ///< Longitude (WGS84, in degrees * 1E7 - int32_t altitude; ///< Altitude (WGS84), in meters * 1000 (positive for up) - uint8_t target_system; ///< System ID -} mavlink_set_gps_global_origin_t; - -#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN 13 -#define MAVLINK_MSG_ID_48_LEN 13 - -#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC 41 -#define MAVLINK_MSG_ID_48_CRC 41 - - - -#define MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN { \ - "SET_GPS_GLOBAL_ORIGIN", \ - 4, \ - { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_gps_global_origin_t, latitude) }, \ - { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_gps_global_origin_t, longitude) }, \ - { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_set_gps_global_origin_t, altitude) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_set_gps_global_origin_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a set_gps_global_origin message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84, in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_uint8_t(buf, 12, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); -#else - mavlink_set_gps_global_origin_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); -#endif -} - -/** - * @brief Pack a set_gps_global_origin message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84, in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_uint8_t(buf, 12, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); -#else - mavlink_set_gps_global_origin_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); -#endif -} - -/** - * @brief Encode a set_gps_global_origin struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_gps_global_origin C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_gps_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin) -{ - return mavlink_msg_set_gps_global_origin_pack(system_id, component_id, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); -} - -/** - * @brief Encode a set_gps_global_origin struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_gps_global_origin C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin) -{ - return mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, chan, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); -} - -/** - * @brief Send a set_gps_global_origin message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param latitude Latitude (WGS84), in degrees * 1E7 - * @param longitude Longitude (WGS84, in degrees * 1E7 - * @param altitude Altitude (WGS84), in meters * 1000 (positive for up) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_uint8_t(buf, 12, target_system); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); -#endif -#else - mavlink_set_gps_global_origin_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.target_system = target_system; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)&packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)&packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_uint8_t(buf, 12, target_system); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); -#endif -#else - mavlink_set_gps_global_origin_t *packet = (mavlink_set_gps_global_origin_t *)msgbuf; - packet->latitude = latitude; - packet->longitude = longitude; - packet->altitude = altitude; - packet->target_system = target_system; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SET_GPS_GLOBAL_ORIGIN UNPACKING - - -/** - * @brief Get field target_system from set_gps_global_origin message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_gps_global_origin_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field latitude from set_gps_global_origin message - * - * @return Latitude (WGS84), in degrees * 1E7 - */ -static inline int32_t mavlink_msg_set_gps_global_origin_get_latitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field longitude from set_gps_global_origin message - * - * @return Longitude (WGS84, in degrees * 1E7 - */ -static inline int32_t mavlink_msg_set_gps_global_origin_get_longitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field altitude from set_gps_global_origin message - * - * @return Altitude (WGS84), in meters * 1000 (positive for up) - */ -static inline int32_t mavlink_msg_set_gps_global_origin_get_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Decode a set_gps_global_origin message into a struct - * - * @param msg The message to decode - * @param set_gps_global_origin C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_gps_global_origin_decode(const mavlink_message_t* msg, mavlink_set_gps_global_origin_t* set_gps_global_origin) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_gps_global_origin->latitude = mavlink_msg_set_gps_global_origin_get_latitude(msg); - set_gps_global_origin->longitude = mavlink_msg_set_gps_global_origin_get_longitude(msg); - set_gps_global_origin->altitude = mavlink_msg_set_gps_global_origin_get_altitude(msg); - set_gps_global_origin->target_system = mavlink_msg_set_gps_global_origin_get_target_system(msg); -#else - memcpy(set_gps_global_origin, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h deleted file mode 100644 index 04dbe6e9a..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE SET_LOCAL_POSITION_SETPOINT PACKING - -#define MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT 50 - -typedef struct __mavlink_set_local_position_setpoint_t -{ - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< Desired yaw angle - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t coordinate_frame; ///< Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU -} mavlink_set_local_position_setpoint_t; - -#define MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN 19 -#define MAVLINK_MSG_ID_50_LEN 19 - -#define MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC 214 -#define MAVLINK_MSG_ID_50_CRC 214 - - - -#define MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT { \ - "SET_LOCAL_POSITION_SETPOINT", \ - 7, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_local_position_setpoint_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_local_position_setpoint_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_local_position_setpoint_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_local_position_setpoint_t, yaw) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_local_position_setpoint_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_local_position_setpoint_t, target_component) }, \ - { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_set_local_position_setpoint_t, coordinate_frame) }, \ - } \ -} - - -/** - * @brief Pack a set_local_position_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_local_position_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#else - mavlink_set_local_position_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a set_local_position_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_local_position_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t coordinate_frame,float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, coordinate_frame); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#else - mavlink_set_local_position_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - packet.coordinate_frame = coordinate_frame; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a set_local_position_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_local_position_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_local_position_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_local_position_setpoint_t* set_local_position_setpoint) -{ - return mavlink_msg_set_local_position_setpoint_pack(system_id, component_id, msg, set_local_position_setpoint->target_system, set_local_position_setpoint->target_component, set_local_position_setpoint->coordinate_frame, set_local_position_setpoint->x, set_local_position_setpoint->y, set_local_position_setpoint->z, set_local_position_setpoint->yaw); -} - -/** - * @brief Encode a set_local_position_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_local_position_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_local_position_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_local_position_setpoint_t* set_local_position_setpoint) -{ - return mavlink_msg_set_local_position_setpoint_pack_chan(system_id, component_id, chan, msg, set_local_position_setpoint->target_system, set_local_position_setpoint->target_component, set_local_position_setpoint->coordinate_frame, set_local_position_setpoint->x, set_local_position_setpoint->y, set_local_position_setpoint->z, set_local_position_setpoint->yaw); -} - -/** - * @brief Send a set_local_position_setpoint message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_local_position_setpoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, coordinate_frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif -#else - mavlink_set_local_position_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - packet.coordinate_frame = coordinate_frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_set_local_position_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, coordinate_frame); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif -#else - mavlink_set_local_position_setpoint_t *packet = (mavlink_set_local_position_setpoint_t *)msgbuf; - packet->x = x; - packet->y = y; - packet->z = z; - packet->yaw = yaw; - packet->target_system = target_system; - packet->target_component = target_component; - packet->coordinate_frame = coordinate_frame; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SET_LOCAL_POSITION_SETPOINT UNPACKING - - -/** - * @brief Get field target_system from set_local_position_setpoint message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_local_position_setpoint_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from set_local_position_setpoint message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_set_local_position_setpoint_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field coordinate_frame from set_local_position_setpoint message - * - * @return Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU - */ -static inline uint8_t mavlink_msg_set_local_position_setpoint_get_coordinate_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 18); -} - -/** - * @brief Get field x from set_local_position_setpoint message - * - * @return x position - */ -static inline float mavlink_msg_set_local_position_setpoint_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from set_local_position_setpoint message - * - * @return y position - */ -static inline float mavlink_msg_set_local_position_setpoint_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from set_local_position_setpoint message - * - * @return z position - */ -static inline float mavlink_msg_set_local_position_setpoint_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from set_local_position_setpoint message - * - * @return Desired yaw angle - */ -static inline float mavlink_msg_set_local_position_setpoint_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a set_local_position_setpoint message into a struct - * - * @param msg The message to decode - * @param set_local_position_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_local_position_setpoint_decode(const mavlink_message_t* msg, mavlink_set_local_position_setpoint_t* set_local_position_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_local_position_setpoint->x = mavlink_msg_set_local_position_setpoint_get_x(msg); - set_local_position_setpoint->y = mavlink_msg_set_local_position_setpoint_get_y(msg); - set_local_position_setpoint->z = mavlink_msg_set_local_position_setpoint_get_z(msg); - set_local_position_setpoint->yaw = mavlink_msg_set_local_position_setpoint_get_yaw(msg); - set_local_position_setpoint->target_system = mavlink_msg_set_local_position_setpoint_get_target_system(msg); - set_local_position_setpoint->target_component = mavlink_msg_set_local_position_setpoint_get_target_component(msg); - set_local_position_setpoint->coordinate_frame = mavlink_msg_set_local_position_setpoint_get_coordinate_frame(msg); -#else - memcpy(set_local_position_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h deleted file mode 100644 index 4b60a4120..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE SET_MODE PACKING - -#define MAVLINK_MSG_ID_SET_MODE 11 - -typedef struct __mavlink_set_mode_t -{ - uint32_t custom_mode; ///< The new autopilot-specific mode. This field can be ignored by an autopilot. - uint8_t target_system; ///< The system setting the mode - uint8_t base_mode; ///< The new base mode -} mavlink_set_mode_t; - -#define MAVLINK_MSG_ID_SET_MODE_LEN 6 -#define MAVLINK_MSG_ID_11_LEN 6 - -#define MAVLINK_MSG_ID_SET_MODE_CRC 89 -#define MAVLINK_MSG_ID_11_CRC 89 - - - -#define MAVLINK_MESSAGE_INFO_SET_MODE { \ - "SET_MODE", \ - 3, \ - { { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_mode_t, custom_mode) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_set_mode_t, target_system) }, \ - { "base_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_set_mode_t, base_mode) }, \ - } \ -} - - -/** - * @brief Pack a set_mode message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system The system setting the mode - * @param base_mode The new base mode - * @param custom_mode The new autopilot-specific mode. This field can be ignored by an autopilot. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t base_mode, uint32_t custom_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_MODE_LEN]; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, base_mode); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MODE_LEN); -#else - mavlink_set_mode_t packet; - packet.custom_mode = custom_mode; - packet.target_system = target_system; - packet.base_mode = base_mode; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MODE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_MODE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_LEN); -#endif -} - -/** - * @brief Pack a set_mode message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system The system setting the mode - * @param base_mode The new base mode - * @param custom_mode The new autopilot-specific mode. This field can be ignored by an autopilot. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t base_mode,uint32_t custom_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_MODE_LEN]; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, base_mode); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MODE_LEN); -#else - mavlink_set_mode_t packet; - packet.custom_mode = custom_mode; - packet.target_system = target_system; - packet.base_mode = base_mode; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MODE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_MODE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MODE_LEN); -#endif -} - -/** - * @brief Encode a set_mode struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_mode C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_mode_t* set_mode) -{ - return mavlink_msg_set_mode_pack(system_id, component_id, msg, set_mode->target_system, set_mode->base_mode, set_mode->custom_mode); -} - -/** - * @brief Encode a set_mode struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_mode C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_mode_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mode_t* set_mode) -{ - return mavlink_msg_set_mode_pack_chan(system_id, component_id, chan, msg, set_mode->target_system, set_mode->base_mode, set_mode->custom_mode); -} - -/** - * @brief Send a set_mode message - * @param chan MAVLink channel to send the message - * - * @param target_system The system setting the mode - * @param base_mode The new base mode - * @param custom_mode The new autopilot-specific mode. This field can be ignored by an autopilot. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target_system, uint8_t base_mode, uint32_t custom_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_MODE_LEN]; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, base_mode); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN); -#endif -#else - mavlink_set_mode_t packet; - packet.custom_mode = custom_mode; - packet.target_system = target_system; - packet.base_mode = base_mode; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)&packet, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)&packet, MAVLINK_MSG_ID_SET_MODE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SET_MODE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_set_mode_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t base_mode, uint32_t custom_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, base_mode); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN); -#endif -#else - mavlink_set_mode_t *packet = (mavlink_set_mode_t *)msgbuf; - packet->custom_mode = custom_mode; - packet->target_system = target_system; - packet->base_mode = base_mode; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)packet, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)packet, MAVLINK_MSG_ID_SET_MODE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SET_MODE UNPACKING - - -/** - * @brief Get field target_system from set_mode message - * - * @return The system setting the mode - */ -static inline uint8_t mavlink_msg_set_mode_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field base_mode from set_mode message - * - * @return The new base mode - */ -static inline uint8_t mavlink_msg_set_mode_get_base_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field custom_mode from set_mode message - * - * @return The new autopilot-specific mode. This field can be ignored by an autopilot. - */ -static inline uint32_t mavlink_msg_set_mode_get_custom_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Decode a set_mode message into a struct - * - * @param msg The message to decode - * @param set_mode C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_mode_decode(const mavlink_message_t* msg, mavlink_set_mode_t* set_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_mode->custom_mode = mavlink_msg_set_mode_get_custom_mode(msg); - set_mode->target_system = mavlink_msg_set_mode_get_target_system(msg); - set_mode->base_mode = mavlink_msg_set_mode_get_base_mode(msg); -#else - memcpy(set_mode, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_MODE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h deleted file mode 100644 index 8c0e2a014..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE SET_QUAD_MOTORS_SETPOINT PACKING - -#define MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT 60 - -typedef struct __mavlink_set_quad_motors_setpoint_t -{ - uint16_t motor_front_nw; ///< Front motor in + configuration, front left motor in x configuration - uint16_t motor_right_ne; ///< Right motor in + configuration, front right motor in x configuration - uint16_t motor_back_se; ///< Back motor in + configuration, back right motor in x configuration - uint16_t motor_left_sw; ///< Left motor in + configuration, back left motor in x configuration - uint8_t target_system; ///< System ID of the system that should set these motor commands -} mavlink_set_quad_motors_setpoint_t; - -#define MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN 9 -#define MAVLINK_MSG_ID_60_LEN 9 - -#define MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC 30 -#define MAVLINK_MSG_ID_60_CRC 30 - - - -#define MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT { \ - "SET_QUAD_MOTORS_SETPOINT", \ - 5, \ - { { "motor_front_nw", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_set_quad_motors_setpoint_t, motor_front_nw) }, \ - { "motor_right_ne", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_set_quad_motors_setpoint_t, motor_right_ne) }, \ - { "motor_back_se", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_set_quad_motors_setpoint_t, motor_back_se) }, \ - { "motor_left_sw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_set_quad_motors_setpoint_t, motor_left_sw) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_set_quad_motors_setpoint_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a set_quad_motors_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID of the system that should set these motor commands - * @param motor_front_nw Front motor in + configuration, front left motor in x configuration - * @param motor_right_ne Right motor in + configuration, front right motor in x configuration - * @param motor_back_se Back motor in + configuration, back right motor in x configuration - * @param motor_left_sw Left motor in + configuration, back left motor in x configuration - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint16_t motor_front_nw, uint16_t motor_right_ne, uint16_t motor_back_se, uint16_t motor_left_sw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN]; - _mav_put_uint16_t(buf, 0, motor_front_nw); - _mav_put_uint16_t(buf, 2, motor_right_ne); - _mav_put_uint16_t(buf, 4, motor_back_se); - _mav_put_uint16_t(buf, 6, motor_left_sw); - _mav_put_uint8_t(buf, 8, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#else - mavlink_set_quad_motors_setpoint_t packet; - packet.motor_front_nw = motor_front_nw; - packet.motor_right_ne = motor_right_ne; - packet.motor_back_se = motor_back_se; - packet.motor_left_sw = motor_left_sw; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a set_quad_motors_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID of the system that should set these motor commands - * @param motor_front_nw Front motor in + configuration, front left motor in x configuration - * @param motor_right_ne Right motor in + configuration, front right motor in x configuration - * @param motor_back_se Back motor in + configuration, back right motor in x configuration - * @param motor_left_sw Left motor in + configuration, back left motor in x configuration - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint16_t motor_front_nw,uint16_t motor_right_ne,uint16_t motor_back_se,uint16_t motor_left_sw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN]; - _mav_put_uint16_t(buf, 0, motor_front_nw); - _mav_put_uint16_t(buf, 2, motor_right_ne); - _mav_put_uint16_t(buf, 4, motor_back_se); - _mav_put_uint16_t(buf, 6, motor_left_sw); - _mav_put_uint8_t(buf, 8, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#else - mavlink_set_quad_motors_setpoint_t packet; - packet.motor_front_nw = motor_front_nw; - packet.motor_right_ne = motor_right_ne; - packet.motor_back_se = motor_back_se; - packet.motor_left_sw = motor_left_sw; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a set_quad_motors_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_quad_motors_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_quad_motors_setpoint_t* set_quad_motors_setpoint) -{ - return mavlink_msg_set_quad_motors_setpoint_pack(system_id, component_id, msg, set_quad_motors_setpoint->target_system, set_quad_motors_setpoint->motor_front_nw, set_quad_motors_setpoint->motor_right_ne, set_quad_motors_setpoint->motor_back_se, set_quad_motors_setpoint->motor_left_sw); -} - -/** - * @brief Encode a set_quad_motors_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_quad_motors_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_motors_setpoint_t* set_quad_motors_setpoint) -{ - return mavlink_msg_set_quad_motors_setpoint_pack_chan(system_id, component_id, chan, msg, set_quad_motors_setpoint->target_system, set_quad_motors_setpoint->motor_front_nw, set_quad_motors_setpoint->motor_right_ne, set_quad_motors_setpoint->motor_back_se, set_quad_motors_setpoint->motor_left_sw); -} - -/** - * @brief Send a set_quad_motors_setpoint message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID of the system that should set these motor commands - * @param motor_front_nw Front motor in + configuration, front left motor in x configuration - * @param motor_right_ne Right motor in + configuration, front right motor in x configuration - * @param motor_back_se Back motor in + configuration, back right motor in x configuration - * @param motor_left_sw Left motor in + configuration, back left motor in x configuration - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_quad_motors_setpoint_send(mavlink_channel_t chan, uint8_t target_system, uint16_t motor_front_nw, uint16_t motor_right_ne, uint16_t motor_back_se, uint16_t motor_left_sw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN]; - _mav_put_uint16_t(buf, 0, motor_front_nw); - _mav_put_uint16_t(buf, 2, motor_right_ne); - _mav_put_uint16_t(buf, 4, motor_back_se); - _mav_put_uint16_t(buf, 6, motor_left_sw); - _mav_put_uint8_t(buf, 8, target_system); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif -#else - mavlink_set_quad_motors_setpoint_t packet; - packet.motor_front_nw = motor_front_nw; - packet.motor_right_ne = motor_right_ne; - packet.motor_back_se = motor_back_se; - packet.motor_left_sw = motor_left_sw; - packet.target_system = target_system; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_set_quad_motors_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint16_t motor_front_nw, uint16_t motor_right_ne, uint16_t motor_back_se, uint16_t motor_left_sw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, motor_front_nw); - _mav_put_uint16_t(buf, 2, motor_right_ne); - _mav_put_uint16_t(buf, 4, motor_back_se); - _mav_put_uint16_t(buf, 6, motor_left_sw); - _mav_put_uint8_t(buf, 8, target_system); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif -#else - mavlink_set_quad_motors_setpoint_t *packet = (mavlink_set_quad_motors_setpoint_t *)msgbuf; - packet->motor_front_nw = motor_front_nw; - packet->motor_right_ne = motor_right_ne; - packet->motor_back_se = motor_back_se; - packet->motor_left_sw = motor_left_sw; - packet->target_system = target_system; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SET_QUAD_MOTORS_SETPOINT UNPACKING - - -/** - * @brief Get field target_system from set_quad_motors_setpoint message - * - * @return System ID of the system that should set these motor commands - */ -static inline uint8_t mavlink_msg_set_quad_motors_setpoint_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field motor_front_nw from set_quad_motors_setpoint message - * - * @return Front motor in + configuration, front left motor in x configuration - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_get_motor_front_nw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field motor_right_ne from set_quad_motors_setpoint message - * - * @return Right motor in + configuration, front right motor in x configuration - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_get_motor_right_ne(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field motor_back_se from set_quad_motors_setpoint message - * - * @return Back motor in + configuration, back right motor in x configuration - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_get_motor_back_se(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field motor_left_sw from set_quad_motors_setpoint message - * - * @return Left motor in + configuration, back left motor in x configuration - */ -static inline uint16_t mavlink_msg_set_quad_motors_setpoint_get_motor_left_sw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Decode a set_quad_motors_setpoint message into a struct - * - * @param msg The message to decode - * @param set_quad_motors_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_quad_motors_setpoint_decode(const mavlink_message_t* msg, mavlink_set_quad_motors_setpoint_t* set_quad_motors_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_quad_motors_setpoint->motor_front_nw = mavlink_msg_set_quad_motors_setpoint_get_motor_front_nw(msg); - set_quad_motors_setpoint->motor_right_ne = mavlink_msg_set_quad_motors_setpoint_get_motor_right_ne(msg); - set_quad_motors_setpoint->motor_back_se = mavlink_msg_set_quad_motors_setpoint_get_motor_back_se(msg); - set_quad_motors_setpoint->motor_left_sw = mavlink_msg_set_quad_motors_setpoint_get_motor_left_sw(msg); - set_quad_motors_setpoint->target_system = mavlink_msg_set_quad_motors_setpoint_get_target_system(msg); -#else - memcpy(set_quad_motors_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h deleted file mode 100644 index c9a084773..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h +++ /dev/null @@ -1,399 +0,0 @@ -// MESSAGE SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST PACKING - -#define MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST 63 - -typedef struct __mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t -{ - int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-INT16_MAX) - int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-INT16_MAX) - int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..UINT16_MAX) - uint8_t group; ///< ID of the quadrotor group (0 - 255, up to 256 groups supported) - uint8_t mode; ///< ID of the flight mode (0 - 255, up to 256 modes supported) - uint8_t led_red[4]; ///< RGB red channel (0-255) - uint8_t led_blue[4]; ///< RGB green channel (0-255) - uint8_t led_green[4]; ///< RGB blue channel (0-255) -} mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t; - -#define MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN 46 -#define MAVLINK_MSG_ID_63_LEN 46 - -#define MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC 130 -#define MAVLINK_MSG_ID_63_CRC 130 - -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_ROLL_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_PITCH_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_YAW_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_THRUST_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_LED_RED_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_LED_BLUE_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_LED_GREEN_LEN 4 - -#define MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST { \ - "SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST", \ - 9, \ - { { "roll", NULL, MAVLINK_TYPE_INT16_T, 4, 0, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_INT16_T, 4, 8, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_INT16_T, 4, 16, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, yaw) }, \ - { "thrust", NULL, MAVLINK_TYPE_UINT16_T, 4, 24, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, thrust) }, \ - { "group", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, group) }, \ - { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, mode) }, \ - { "led_red", NULL, MAVLINK_TYPE_UINT8_T, 4, 34, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, led_red) }, \ - { "led_blue", NULL, MAVLINK_TYPE_UINT8_T, 4, 38, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, led_blue) }, \ - { "led_green", NULL, MAVLINK_TYPE_UINT8_T, 4, 42, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, led_green) }, \ - } \ -} - - -/** - * @brief Pack a set_quad_swarm_led_roll_pitch_yaw_thrust message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) - * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param led_red RGB red channel (0-255) - * @param led_blue RGB green channel (0-255) - * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) - * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t group, uint8_t mode, const uint8_t *led_red, const uint8_t *led_blue, const uint8_t *led_green, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); - _mav_put_uint8_t_array(buf, 34, led_red, 4); - _mav_put_uint8_t_array(buf, 38, led_blue, 4); - _mav_put_uint8_t_array(buf, 42, led_green, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#else - mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t packet; - packet.group = group; - packet.mode = mode; - mav_array_memcpy(packet.roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet.pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet.yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet.thrust, thrust, sizeof(uint16_t)*4); - mav_array_memcpy(packet.led_red, led_red, sizeof(uint8_t)*4); - mav_array_memcpy(packet.led_blue, led_blue, sizeof(uint8_t)*4); - mav_array_memcpy(packet.led_green, led_green, sizeof(uint8_t)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} - -/** - * @brief Pack a set_quad_swarm_led_roll_pitch_yaw_thrust message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) - * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param led_red RGB red channel (0-255) - * @param led_blue RGB green channel (0-255) - * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) - * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t group,uint8_t mode,const uint8_t *led_red,const uint8_t *led_blue,const uint8_t *led_green,const int16_t *roll,const int16_t *pitch,const int16_t *yaw,const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); - _mav_put_uint8_t_array(buf, 34, led_red, 4); - _mav_put_uint8_t_array(buf, 38, led_blue, 4); - _mav_put_uint8_t_array(buf, 42, led_green, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#else - mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t packet; - packet.group = group; - packet.mode = mode; - mav_array_memcpy(packet.roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet.pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet.yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet.thrust, thrust, sizeof(uint16_t)*4); - mav_array_memcpy(packet.led_red, led_red, sizeof(uint8_t)*4); - mav_array_memcpy(packet.led_blue, led_blue, sizeof(uint8_t)*4); - mav_array_memcpy(packet.led_green, led_green, sizeof(uint8_t)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} - -/** - * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_quad_swarm_led_roll_pitch_yaw_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t* set_quad_swarm_led_roll_pitch_yaw_thrust) -{ - return mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_quad_swarm_led_roll_pitch_yaw_thrust->group, set_quad_swarm_led_roll_pitch_yaw_thrust->mode, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green, set_quad_swarm_led_roll_pitch_yaw_thrust->roll, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust); -} - -/** - * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_quad_swarm_led_roll_pitch_yaw_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t* set_quad_swarm_led_roll_pitch_yaw_thrust) -{ - return mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_quad_swarm_led_roll_pitch_yaw_thrust->group, set_quad_swarm_led_roll_pitch_yaw_thrust->mode, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green, set_quad_swarm_led_roll_pitch_yaw_thrust->roll, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust); -} - -/** - * @brief Send a set_quad_swarm_led_roll_pitch_yaw_thrust message - * @param chan MAVLink channel to send the message - * - * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) - * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param led_red RGB red channel (0-255) - * @param led_blue RGB green channel (0-255) - * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) - * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send(mavlink_channel_t chan, uint8_t group, uint8_t mode, const uint8_t *led_red, const uint8_t *led_blue, const uint8_t *led_green, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); - _mav_put_uint8_t_array(buf, 34, led_red, 4); - _mav_put_uint8_t_array(buf, 38, led_blue, 4); - _mav_put_uint8_t_array(buf, 42, led_green, 4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#else - mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t packet; - packet.group = group; - packet.mode = mode; - mav_array_memcpy(packet.roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet.pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet.yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet.thrust, thrust, sizeof(uint16_t)*4); - mav_array_memcpy(packet.led_red, led_red, sizeof(uint8_t)*4); - mav_array_memcpy(packet.led_blue, led_blue, sizeof(uint8_t)*4); - mav_array_memcpy(packet.led_green, led_green, sizeof(uint8_t)*4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t group, uint8_t mode, const uint8_t *led_red, const uint8_t *led_blue, const uint8_t *led_green, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); - _mav_put_uint8_t_array(buf, 34, led_red, 4); - _mav_put_uint8_t_array(buf, 38, led_blue, 4); - _mav_put_uint8_t_array(buf, 42, led_green, 4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#else - mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t *packet = (mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t *)msgbuf; - packet->group = group; - packet->mode = mode; - mav_array_memcpy(packet->roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet->pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet->yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet->thrust, thrust, sizeof(uint16_t)*4); - mav_array_memcpy(packet->led_red, led_red, sizeof(uint8_t)*4); - mav_array_memcpy(packet->led_blue, led_blue, sizeof(uint8_t)*4); - mav_array_memcpy(packet->led_green, led_green, sizeof(uint8_t)*4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST UNPACKING - - -/** - * @brief Get field group from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return ID of the quadrotor group (0 - 255, up to 256 groups supported) - */ -static inline uint8_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_group(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field mode from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return ID of the flight mode (0 - 255, up to 256 modes supported) - */ -static inline uint8_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 33); -} - -/** - * @brief Get field led_red from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return RGB red channel (0-255) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_red(const mavlink_message_t* msg, uint8_t *led_red) -{ - return _MAV_RETURN_uint8_t_array(msg, led_red, 4, 34); -} - -/** - * @brief Get field led_blue from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return RGB green channel (0-255) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_blue(const mavlink_message_t* msg, uint8_t *led_blue) -{ - return _MAV_RETURN_uint8_t_array(msg, led_blue, 4, 38); -} - -/** - * @brief Get field led_green from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return RGB blue channel (0-255) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_green(const mavlink_message_t* msg, uint8_t *led_green) -{ - return _MAV_RETURN_uint8_t_array(msg, led_green, 4, 42); -} - -/** - * @brief Get field roll from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return Desired roll angle in radians +-PI (+-INT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg, int16_t *roll) -{ - return _MAV_RETURN_int16_t_array(msg, roll, 4, 0); -} - -/** - * @brief Get field pitch from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return Desired pitch angle in radians +-PI (+-INT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg, int16_t *pitch) -{ - return _MAV_RETURN_int16_t_array(msg, pitch, 4, 8); -} - -/** - * @brief Get field yaw from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg, int16_t *yaw) -{ - return _MAV_RETURN_int16_t_array(msg, yaw, 4, 16); -} - -/** - * @brief Get field thrust from set_quad_swarm_led_roll_pitch_yaw_thrust message - * - * @return Collective thrust, scaled to uint16 (0..UINT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg, uint16_t *thrust) -{ - return _MAV_RETURN_uint16_t_array(msg, thrust, 4, 24); -} - -/** - * @brief Decode a set_quad_swarm_led_roll_pitch_yaw_thrust message into a struct - * - * @param msg The message to decode - * @param set_quad_swarm_led_roll_pitch_yaw_thrust C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t* set_quad_swarm_led_roll_pitch_yaw_thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_roll(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->roll); - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_pitch(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch); - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_yaw(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw); - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_thrust(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust); - set_quad_swarm_led_roll_pitch_yaw_thrust->group = mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_group(msg); - set_quad_swarm_led_roll_pitch_yaw_thrust->mode = mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_mode(msg); - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_red(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red); - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_blue(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue); - mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_green(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green); -#else - memcpy(set_quad_swarm_led_roll_pitch_yaw_thrust, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h deleted file mode 100644 index 338bf0e43..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h +++ /dev/null @@ -1,324 +0,0 @@ -// MESSAGE SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST PACKING - -#define MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST 61 - -typedef struct __mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t -{ - int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-INT16_MAX) - int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-INT16_MAX) - int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..UINT16_MAX) - uint8_t group; ///< ID of the quadrotor group (0 - 255, up to 256 groups supported) - uint8_t mode; ///< ID of the flight mode (0 - 255, up to 256 modes supported) -} mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t; - -#define MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN 34 -#define MAVLINK_MSG_ID_61_LEN 34 - -#define MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC 240 -#define MAVLINK_MSG_ID_61_CRC 240 - -#define MAVLINK_MSG_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_FIELD_ROLL_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_FIELD_PITCH_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_FIELD_YAW_LEN 4 -#define MAVLINK_MSG_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_FIELD_THRUST_LEN 4 - -#define MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST { \ - "SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST", \ - 6, \ - { { "roll", NULL, MAVLINK_TYPE_INT16_T, 4, 0, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_INT16_T, 4, 8, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_INT16_T, 4, 16, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, yaw) }, \ - { "thrust", NULL, MAVLINK_TYPE_UINT16_T, 4, 24, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, thrust) }, \ - { "group", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, group) }, \ - { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, mode) }, \ - } \ -} - - -/** - * @brief Pack a set_quad_swarm_roll_pitch_yaw_thrust message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) - * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) - * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t group, uint8_t mode, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#else - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t packet; - packet.group = group; - packet.mode = mode; - mav_array_memcpy(packet.roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet.pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet.yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet.thrust, thrust, sizeof(uint16_t)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} - -/** - * @brief Pack a set_quad_swarm_roll_pitch_yaw_thrust message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) - * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) - * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t group,uint8_t mode,const int16_t *roll,const int16_t *pitch,const int16_t *yaw,const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#else - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t packet; - packet.group = group; - packet.mode = mode; - mav_array_memcpy(packet.roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet.pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet.yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet.thrust, thrust, sizeof(uint16_t)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} - -/** - * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_quad_swarm_roll_pitch_yaw_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t* set_quad_swarm_roll_pitch_yaw_thrust) -{ - return mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_quad_swarm_roll_pitch_yaw_thrust->group, set_quad_swarm_roll_pitch_yaw_thrust->mode, set_quad_swarm_roll_pitch_yaw_thrust->roll, set_quad_swarm_roll_pitch_yaw_thrust->pitch, set_quad_swarm_roll_pitch_yaw_thrust->yaw, set_quad_swarm_roll_pitch_yaw_thrust->thrust); -} - -/** - * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_quad_swarm_roll_pitch_yaw_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t* set_quad_swarm_roll_pitch_yaw_thrust) -{ - return mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_quad_swarm_roll_pitch_yaw_thrust->group, set_quad_swarm_roll_pitch_yaw_thrust->mode, set_quad_swarm_roll_pitch_yaw_thrust->roll, set_quad_swarm_roll_pitch_yaw_thrust->pitch, set_quad_swarm_roll_pitch_yaw_thrust->yaw, set_quad_swarm_roll_pitch_yaw_thrust->thrust); -} - -/** - * @brief Send a set_quad_swarm_roll_pitch_yaw_thrust message - * @param chan MAVLink channel to send the message - * - * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) - * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) - * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_send(mavlink_channel_t chan, uint8_t group, uint8_t mode, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#else - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t packet; - packet.group = group; - packet.mode = mode; - mav_array_memcpy(packet.roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet.pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet.yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet.thrust, thrust, sizeof(uint16_t)*4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t group, uint8_t mode, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 32, group); - _mav_put_uint8_t(buf, 33, mode); - _mav_put_int16_t_array(buf, 0, roll, 4); - _mav_put_int16_t_array(buf, 8, pitch, 4); - _mav_put_int16_t_array(buf, 16, yaw, 4); - _mav_put_uint16_t_array(buf, 24, thrust, 4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#else - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t *packet = (mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t *)msgbuf; - packet->group = group; - packet->mode = mode; - mav_array_memcpy(packet->roll, roll, sizeof(int16_t)*4); - mav_array_memcpy(packet->pitch, pitch, sizeof(int16_t)*4); - mav_array_memcpy(packet->yaw, yaw, sizeof(int16_t)*4); - mav_array_memcpy(packet->thrust, thrust, sizeof(uint16_t)*4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST UNPACKING - - -/** - * @brief Get field group from set_quad_swarm_roll_pitch_yaw_thrust message - * - * @return ID of the quadrotor group (0 - 255, up to 256 groups supported) - */ -static inline uint8_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_group(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field mode from set_quad_swarm_roll_pitch_yaw_thrust message - * - * @return ID of the flight mode (0 - 255, up to 256 modes supported) - */ -static inline uint8_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 33); -} - -/** - * @brief Get field roll from set_quad_swarm_roll_pitch_yaw_thrust message - * - * @return Desired roll angle in radians +-PI (+-INT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg, int16_t *roll) -{ - return _MAV_RETURN_int16_t_array(msg, roll, 4, 0); -} - -/** - * @brief Get field pitch from set_quad_swarm_roll_pitch_yaw_thrust message - * - * @return Desired pitch angle in radians +-PI (+-INT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg, int16_t *pitch) -{ - return _MAV_RETURN_int16_t_array(msg, pitch, 4, 8); -} - -/** - * @brief Get field yaw from set_quad_swarm_roll_pitch_yaw_thrust message - * - * @return Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg, int16_t *yaw) -{ - return _MAV_RETURN_int16_t_array(msg, yaw, 4, 16); -} - -/** - * @brief Get field thrust from set_quad_swarm_roll_pitch_yaw_thrust message - * - * @return Collective thrust, scaled to uint16 (0..UINT16_MAX) - */ -static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg, uint16_t *thrust) -{ - return _MAV_RETURN_uint16_t_array(msg, thrust, 4, 24); -} - -/** - * @brief Decode a set_quad_swarm_roll_pitch_yaw_thrust message into a struct - * - * @param msg The message to decode - * @param set_quad_swarm_roll_pitch_yaw_thrust C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t* set_quad_swarm_roll_pitch_yaw_thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll(msg, set_quad_swarm_roll_pitch_yaw_thrust->roll); - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitch(msg, set_quad_swarm_roll_pitch_yaw_thrust->pitch); - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw(msg, set_quad_swarm_roll_pitch_yaw_thrust->yaw); - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_thrust(msg, set_quad_swarm_roll_pitch_yaw_thrust->thrust); - set_quad_swarm_roll_pitch_yaw_thrust->group = mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_group(msg); - set_quad_swarm_roll_pitch_yaw_thrust->mode = mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_mode(msg); -#else - memcpy(set_quad_swarm_roll_pitch_yaw_thrust, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h deleted file mode 100644 index 4d0e5931f..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST PACKING - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST 57 - -typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_t -{ - float roll_speed; ///< Desired roll angular speed in rad/s - float pitch_speed; ///< Desired pitch angular speed in rad/s - float yaw_speed; ///< Desired yaw angular speed in rad/s - float thrust; ///< Collective thrust, normalized to 0 .. 1 - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_set_roll_pitch_yaw_speed_thrust_t; - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN 18 -#define MAVLINK_MSG_ID_57_LEN 18 - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC 24 -#define MAVLINK_MSG_ID_57_CRC 24 - - - -#define MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST { \ - "SET_ROLL_PITCH_YAW_SPEED_THRUST", \ - 6, \ - { { "roll_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, roll_speed) }, \ - { "pitch_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, pitch_speed) }, \ - { "yaw_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, yaw_speed) }, \ - { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, thrust) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a set_roll_pitch_yaw_speed_thrust message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN]; - _mav_put_float(buf, 0, roll_speed); - _mav_put_float(buf, 4, pitch_speed); - _mav_put_float(buf, 8, yaw_speed); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#else - mavlink_set_roll_pitch_yaw_speed_thrust_t packet; - packet.roll_speed = roll_speed; - packet.pitch_speed = pitch_speed; - packet.yaw_speed = yaw_speed; - packet.thrust = thrust; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif -} - -/** - * @brief Pack a set_roll_pitch_yaw_speed_thrust message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,float roll_speed,float pitch_speed,float yaw_speed,float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN]; - _mav_put_float(buf, 0, roll_speed); - _mav_put_float(buf, 4, pitch_speed); - _mav_put_float(buf, 8, yaw_speed); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#else - mavlink_set_roll_pitch_yaw_speed_thrust_t packet; - packet.roll_speed = roll_speed; - packet.pitch_speed = pitch_speed; - packet.yaw_speed = yaw_speed; - packet.thrust = thrust; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif -} - -/** - * @brief Encode a set_roll_pitch_yaw_speed_thrust struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) -{ - return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); -} - -/** - * @brief Encode a set_roll_pitch_yaw_speed_thrust struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) -{ - return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(system_id, component_id, chan, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); -} - -/** - * @brief Send a set_roll_pitch_yaw_speed_thrust message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN]; - _mav_put_float(buf, 0, roll_speed); - _mav_put_float(buf, 4, pitch_speed); - _mav_put_float(buf, 8, yaw_speed); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif -#else - mavlink_set_roll_pitch_yaw_speed_thrust_t packet; - packet.roll_speed = roll_speed; - packet.pitch_speed = pitch_speed; - packet.yaw_speed = yaw_speed; - packet.thrust = thrust; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, roll_speed); - _mav_put_float(buf, 4, pitch_speed); - _mav_put_float(buf, 8, yaw_speed); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif -#else - mavlink_set_roll_pitch_yaw_speed_thrust_t *packet = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)msgbuf; - packet->roll_speed = roll_speed; - packet->pitch_speed = pitch_speed; - packet->yaw_speed = yaw_speed; - packet->thrust = thrust; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST UNPACKING - - -/** - * @brief Get field target_system from set_roll_pitch_yaw_speed_thrust message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from set_roll_pitch_yaw_speed_thrust message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field roll_speed from set_roll_pitch_yaw_speed_thrust message - * - * @return Desired roll angular speed in rad/s - */ -static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field pitch_speed from set_roll_pitch_yaw_speed_thrust message - * - * @return Desired pitch angular speed in rad/s - */ -static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field yaw_speed from set_roll_pitch_yaw_speed_thrust message - * - * @return Desired yaw angular speed in rad/s - */ -static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field thrust from set_roll_pitch_yaw_speed_thrust message - * - * @return Collective thrust, normalized to 0 .. 1 - */ -static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a set_roll_pitch_yaw_speed_thrust message into a struct - * - * @param msg The message to decode - * @param set_roll_pitch_yaw_speed_thrust C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_roll_pitch_yaw_speed_thrust->roll_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(msg); - set_roll_pitch_yaw_speed_thrust->pitch_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(msg); - set_roll_pitch_yaw_speed_thrust->yaw_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(msg); - set_roll_pitch_yaw_speed_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(msg); - set_roll_pitch_yaw_speed_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(msg); - set_roll_pitch_yaw_speed_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(msg); -#else - memcpy(set_roll_pitch_yaw_speed_thrust, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h deleted file mode 100644 index 21e0a43f2..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE SET_ROLL_PITCH_YAW_THRUST PACKING - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST 56 - -typedef struct __mavlink_set_roll_pitch_yaw_thrust_t -{ - float roll; ///< Desired roll angle in radians - float pitch; ///< Desired pitch angle in radians - float yaw; ///< Desired yaw angle in radians - float thrust; ///< Collective thrust, normalized to 0 .. 1 - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_set_roll_pitch_yaw_thrust_t; - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN 18 -#define MAVLINK_MSG_ID_56_LEN 18 - -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC 100 -#define MAVLINK_MSG_ID_56_CRC 100 - - - -#define MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST { \ - "SET_ROLL_PITCH_YAW_THRUST", \ - 6, \ - { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, yaw) }, \ - { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, thrust) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a set_roll_pitch_yaw_thrust message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#else - mavlink_set_roll_pitch_yaw_thrust_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} - -/** - * @brief Pack a set_roll_pitch_yaw_thrust message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,float roll,float pitch,float yaw,float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#else - mavlink_set_roll_pitch_yaw_thrust_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} - -/** - * @brief Encode a set_roll_pitch_yaw_thrust struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_roll_pitch_yaw_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) -{ - return mavlink_msg_set_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); -} - -/** - * @brief Encode a set_roll_pitch_yaw_thrust struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_roll_pitch_yaw_thrust C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) -{ - return mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); -} - -/** - * @brief Send a set_roll_pitch_yaw_thrust message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#else - mavlink_set_roll_pitch_yaw_thrust_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)&packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#else - mavlink_set_roll_pitch_yaw_thrust_t *packet = (mavlink_set_roll_pitch_yaw_thrust_t *)msgbuf; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - packet->thrust = thrust; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SET_ROLL_PITCH_YAW_THRUST UNPACKING - - -/** - * @brief Get field target_system from set_roll_pitch_yaw_thrust message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from set_roll_pitch_yaw_thrust message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field roll from set_roll_pitch_yaw_thrust message - * - * @return Desired roll angle in radians - */ -static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field pitch from set_roll_pitch_yaw_thrust message - * - * @return Desired pitch angle in radians - */ -static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field yaw from set_roll_pitch_yaw_thrust message - * - * @return Desired yaw angle in radians - */ -static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field thrust from set_roll_pitch_yaw_thrust message - * - * @return Collective thrust, normalized to 0 .. 1 - */ -static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a set_roll_pitch_yaw_thrust message into a struct - * - * @param msg The message to decode - * @param set_roll_pitch_yaw_thrust C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_roll_pitch_yaw_thrust->roll = mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(msg); - set_roll_pitch_yaw_thrust->pitch = mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(msg); - set_roll_pitch_yaw_thrust->yaw = mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(msg); - set_roll_pitch_yaw_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(msg); - set_roll_pitch_yaw_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(msg); - set_roll_pitch_yaw_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(msg); -#else - memcpy(set_roll_pitch_yaw_thrust, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h deleted file mode 100644 index 90b63427b..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE SETPOINT_6DOF PACKING - -#define MAVLINK_MSG_ID_SETPOINT_6DOF 149 - -typedef struct __mavlink_setpoint_6dof_t -{ - float trans_x; ///< Translational Component in x - float trans_y; ///< Translational Component in y - float trans_z; ///< Translational Component in z - float rot_x; ///< Rotational Component in x - float rot_y; ///< Rotational Component in y - float rot_z; ///< Rotational Component in z - uint8_t target_system; ///< System ID -} mavlink_setpoint_6dof_t; - -#define MAVLINK_MSG_ID_SETPOINT_6DOF_LEN 25 -#define MAVLINK_MSG_ID_149_LEN 25 - -#define MAVLINK_MSG_ID_SETPOINT_6DOF_CRC 15 -#define MAVLINK_MSG_ID_149_CRC 15 - - - -#define MAVLINK_MESSAGE_INFO_SETPOINT_6DOF { \ - "SETPOINT_6DOF", \ - 7, \ - { { "trans_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_setpoint_6dof_t, trans_x) }, \ - { "trans_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_setpoint_6dof_t, trans_y) }, \ - { "trans_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_setpoint_6dof_t, trans_z) }, \ - { "rot_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_setpoint_6dof_t, rot_x) }, \ - { "rot_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_setpoint_6dof_t, rot_y) }, \ - { "rot_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_setpoint_6dof_t, rot_z) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_setpoint_6dof_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a setpoint_6dof message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_setpoint_6dof_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SETPOINT_6DOF_LEN]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#else - mavlink_setpoint_6dof_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SETPOINT_6DOF; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif -} - -/** - * @brief Pack a setpoint_6dof message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_setpoint_6dof_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,float trans_x,float trans_y,float trans_z,float rot_x,float rot_y,float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SETPOINT_6DOF_LEN]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#else - mavlink_setpoint_6dof_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SETPOINT_6DOF; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif -} - -/** - * @brief Encode a setpoint_6dof struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param setpoint_6dof C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_setpoint_6dof_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_setpoint_6dof_t* setpoint_6dof) -{ - return mavlink_msg_setpoint_6dof_pack(system_id, component_id, msg, setpoint_6dof->target_system, setpoint_6dof->trans_x, setpoint_6dof->trans_y, setpoint_6dof->trans_z, setpoint_6dof->rot_x, setpoint_6dof->rot_y, setpoint_6dof->rot_z); -} - -/** - * @brief Encode a setpoint_6dof struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param setpoint_6dof C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_setpoint_6dof_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setpoint_6dof_t* setpoint_6dof) -{ - return mavlink_msg_setpoint_6dof_pack_chan(system_id, component_id, chan, msg, setpoint_6dof->target_system, setpoint_6dof->trans_x, setpoint_6dof->trans_y, setpoint_6dof->trans_z, setpoint_6dof->rot_x, setpoint_6dof->rot_y, setpoint_6dof->rot_z); -} - -/** - * @brief Send a setpoint_6dof message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_setpoint_6dof_send(mavlink_channel_t chan, uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SETPOINT_6DOF_LEN]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif -#else - mavlink_setpoint_6dof_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)&packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)&packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SETPOINT_6DOF_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_setpoint_6dof_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif -#else - mavlink_setpoint_6dof_t *packet = (mavlink_setpoint_6dof_t *)msgbuf; - packet->trans_x = trans_x; - packet->trans_y = trans_y; - packet->trans_z = trans_z; - packet->rot_x = rot_x; - packet->rot_y = rot_y; - packet->rot_z = rot_z; - packet->target_system = target_system; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SETPOINT_6DOF UNPACKING - - -/** - * @brief Get field target_system from setpoint_6dof message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_setpoint_6dof_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 24); -} - -/** - * @brief Get field trans_x from setpoint_6dof message - * - * @return Translational Component in x - */ -static inline float mavlink_msg_setpoint_6dof_get_trans_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field trans_y from setpoint_6dof message - * - * @return Translational Component in y - */ -static inline float mavlink_msg_setpoint_6dof_get_trans_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field trans_z from setpoint_6dof message - * - * @return Translational Component in z - */ -static inline float mavlink_msg_setpoint_6dof_get_trans_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field rot_x from setpoint_6dof message - * - * @return Rotational Component in x - */ -static inline float mavlink_msg_setpoint_6dof_get_rot_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field rot_y from setpoint_6dof message - * - * @return Rotational Component in y - */ -static inline float mavlink_msg_setpoint_6dof_get_rot_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field rot_z from setpoint_6dof message - * - * @return Rotational Component in z - */ -static inline float mavlink_msg_setpoint_6dof_get_rot_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a setpoint_6dof message into a struct - * - * @param msg The message to decode - * @param setpoint_6dof C-struct to decode the message contents into - */ -static inline void mavlink_msg_setpoint_6dof_decode(const mavlink_message_t* msg, mavlink_setpoint_6dof_t* setpoint_6dof) -{ -#if MAVLINK_NEED_BYTE_SWAP - setpoint_6dof->trans_x = mavlink_msg_setpoint_6dof_get_trans_x(msg); - setpoint_6dof->trans_y = mavlink_msg_setpoint_6dof_get_trans_y(msg); - setpoint_6dof->trans_z = mavlink_msg_setpoint_6dof_get_trans_z(msg); - setpoint_6dof->rot_x = mavlink_msg_setpoint_6dof_get_rot_x(msg); - setpoint_6dof->rot_y = mavlink_msg_setpoint_6dof_get_rot_y(msg); - setpoint_6dof->rot_z = mavlink_msg_setpoint_6dof_get_rot_z(msg); - setpoint_6dof->target_system = mavlink_msg_setpoint_6dof_get_target_system(msg); -#else - memcpy(setpoint_6dof, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h deleted file mode 100644 index 308a211cd..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h +++ /dev/null @@ -1,401 +0,0 @@ -// MESSAGE SETPOINT_8DOF PACKING - -#define MAVLINK_MSG_ID_SETPOINT_8DOF 148 - -typedef struct __mavlink_setpoint_8dof_t -{ - float val1; ///< Value 1 - float val2; ///< Value 2 - float val3; ///< Value 3 - float val4; ///< Value 4 - float val5; ///< Value 5 - float val6; ///< Value 6 - float val7; ///< Value 7 - float val8; ///< Value 8 - uint8_t target_system; ///< System ID -} mavlink_setpoint_8dof_t; - -#define MAVLINK_MSG_ID_SETPOINT_8DOF_LEN 33 -#define MAVLINK_MSG_ID_148_LEN 33 - -#define MAVLINK_MSG_ID_SETPOINT_8DOF_CRC 241 -#define MAVLINK_MSG_ID_148_CRC 241 - - - -#define MAVLINK_MESSAGE_INFO_SETPOINT_8DOF { \ - "SETPOINT_8DOF", \ - 9, \ - { { "val1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_setpoint_8dof_t, val1) }, \ - { "val2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_setpoint_8dof_t, val2) }, \ - { "val3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_setpoint_8dof_t, val3) }, \ - { "val4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_setpoint_8dof_t, val4) }, \ - { "val5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_setpoint_8dof_t, val5) }, \ - { "val6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_setpoint_8dof_t, val6) }, \ - { "val7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_setpoint_8dof_t, val7) }, \ - { "val8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_setpoint_8dof_t, val8) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_setpoint_8dof_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a setpoint_8dof message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_setpoint_8dof_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SETPOINT_8DOF_LEN]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#else - mavlink_setpoint_8dof_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SETPOINT_8DOF; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif -} - -/** - * @brief Pack a setpoint_8dof message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_setpoint_8dof_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,float val1,float val2,float val3,float val4,float val5,float val6,float val7,float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SETPOINT_8DOF_LEN]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#else - mavlink_setpoint_8dof_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SETPOINT_8DOF; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif -} - -/** - * @brief Encode a setpoint_8dof struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param setpoint_8dof C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_setpoint_8dof_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_setpoint_8dof_t* setpoint_8dof) -{ - return mavlink_msg_setpoint_8dof_pack(system_id, component_id, msg, setpoint_8dof->target_system, setpoint_8dof->val1, setpoint_8dof->val2, setpoint_8dof->val3, setpoint_8dof->val4, setpoint_8dof->val5, setpoint_8dof->val6, setpoint_8dof->val7, setpoint_8dof->val8); -} - -/** - * @brief Encode a setpoint_8dof struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param setpoint_8dof C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_setpoint_8dof_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setpoint_8dof_t* setpoint_8dof) -{ - return mavlink_msg_setpoint_8dof_pack_chan(system_id, component_id, chan, msg, setpoint_8dof->target_system, setpoint_8dof->val1, setpoint_8dof->val2, setpoint_8dof->val3, setpoint_8dof->val4, setpoint_8dof->val5, setpoint_8dof->val6, setpoint_8dof->val7, setpoint_8dof->val8); -} - -/** - * @brief Send a setpoint_8dof message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_setpoint_8dof_send(mavlink_channel_t chan, uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SETPOINT_8DOF_LEN]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif -#else - mavlink_setpoint_8dof_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)&packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)&packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SETPOINT_8DOF_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_setpoint_8dof_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif -#else - mavlink_setpoint_8dof_t *packet = (mavlink_setpoint_8dof_t *)msgbuf; - packet->val1 = val1; - packet->val2 = val2; - packet->val3 = val3; - packet->val4 = val4; - packet->val5 = val5; - packet->val6 = val6; - packet->val7 = val7; - packet->val8 = val8; - packet->target_system = target_system; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SETPOINT_8DOF UNPACKING - - -/** - * @brief Get field target_system from setpoint_8dof message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_setpoint_8dof_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field val1 from setpoint_8dof message - * - * @return Value 1 - */ -static inline float mavlink_msg_setpoint_8dof_get_val1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field val2 from setpoint_8dof message - * - * @return Value 2 - */ -static inline float mavlink_msg_setpoint_8dof_get_val2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field val3 from setpoint_8dof message - * - * @return Value 3 - */ -static inline float mavlink_msg_setpoint_8dof_get_val3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field val4 from setpoint_8dof message - * - * @return Value 4 - */ -static inline float mavlink_msg_setpoint_8dof_get_val4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field val5 from setpoint_8dof message - * - * @return Value 5 - */ -static inline float mavlink_msg_setpoint_8dof_get_val5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field val6 from setpoint_8dof message - * - * @return Value 6 - */ -static inline float mavlink_msg_setpoint_8dof_get_val6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field val7 from setpoint_8dof message - * - * @return Value 7 - */ -static inline float mavlink_msg_setpoint_8dof_get_val7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field val8 from setpoint_8dof message - * - * @return Value 8 - */ -static inline float mavlink_msg_setpoint_8dof_get_val8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a setpoint_8dof message into a struct - * - * @param msg The message to decode - * @param setpoint_8dof C-struct to decode the message contents into - */ -static inline void mavlink_msg_setpoint_8dof_decode(const mavlink_message_t* msg, mavlink_setpoint_8dof_t* setpoint_8dof) -{ -#if MAVLINK_NEED_BYTE_SWAP - setpoint_8dof->val1 = mavlink_msg_setpoint_8dof_get_val1(msg); - setpoint_8dof->val2 = mavlink_msg_setpoint_8dof_get_val2(msg); - setpoint_8dof->val3 = mavlink_msg_setpoint_8dof_get_val3(msg); - setpoint_8dof->val4 = mavlink_msg_setpoint_8dof_get_val4(msg); - setpoint_8dof->val5 = mavlink_msg_setpoint_8dof_get_val5(msg); - setpoint_8dof->val6 = mavlink_msg_setpoint_8dof_get_val6(msg); - setpoint_8dof->val7 = mavlink_msg_setpoint_8dof_get_val7(msg); - setpoint_8dof->val8 = mavlink_msg_setpoint_8dof_get_val8(msg); - setpoint_8dof->target_system = mavlink_msg_setpoint_8dof_get_target_system(msg); -#else - memcpy(setpoint_8dof, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h deleted file mode 100644 index db3a92642..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h +++ /dev/null @@ -1,689 +0,0 @@ -// MESSAGE SIM_STATE PACKING - -#define MAVLINK_MSG_ID_SIM_STATE 108 - -typedef struct __mavlink_sim_state_t -{ - float q1; ///< True attitude quaternion component 1 - float q2; ///< True attitude quaternion component 2 - float q3; ///< True attitude quaternion component 3 - float q4; ///< True attitude quaternion component 4 - float roll; ///< Attitude roll expressed as Euler angles, not recommended except for human-readable outputs - float pitch; ///< Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs - float yaw; ///< Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs - float xacc; ///< X acceleration m/s/s - float yacc; ///< Y acceleration m/s/s - float zacc; ///< Z acceleration m/s/s - float xgyro; ///< Angular speed around X axis rad/s - float ygyro; ///< Angular speed around Y axis rad/s - float zgyro; ///< Angular speed around Z axis rad/s - float lat; ///< Latitude in degrees - float lon; ///< Longitude in degrees - float alt; ///< Altitude in meters - float std_dev_horz; ///< Horizontal position standard deviation - float std_dev_vert; ///< Vertical position standard deviation - float vn; ///< True velocity in m/s in NORTH direction in earth-fixed NED frame - float ve; ///< True velocity in m/s in EAST direction in earth-fixed NED frame - float vd; ///< True velocity in m/s in DOWN direction in earth-fixed NED frame -} mavlink_sim_state_t; - -#define MAVLINK_MSG_ID_SIM_STATE_LEN 84 -#define MAVLINK_MSG_ID_108_LEN 84 - -#define MAVLINK_MSG_ID_SIM_STATE_CRC 32 -#define MAVLINK_MSG_ID_108_CRC 32 - - - -#define MAVLINK_MESSAGE_INFO_SIM_STATE { \ - "SIM_STATE", \ - 21, \ - { { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sim_state_t, q1) }, \ - { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_sim_state_t, q2) }, \ - { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_sim_state_t, q3) }, \ - { "q4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sim_state_t, q4) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sim_state_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sim_state_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_sim_state_t, yaw) }, \ - { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_sim_state_t, xacc) }, \ - { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_sim_state_t, yacc) }, \ - { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_sim_state_t, zacc) }, \ - { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_sim_state_t, xgyro) }, \ - { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_sim_state_t, ygyro) }, \ - { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_sim_state_t, zgyro) }, \ - { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_sim_state_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_sim_state_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_sim_state_t, alt) }, \ - { "std_dev_horz", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_sim_state_t, std_dev_horz) }, \ - { "std_dev_vert", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_sim_state_t, std_dev_vert) }, \ - { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_sim_state_t, vn) }, \ - { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_sim_state_t, ve) }, \ - { "vd", NULL, MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_sim_state_t, vd) }, \ - } \ -} - - -/** - * @brief Pack a sim_state message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param q1 True attitude quaternion component 1 - * @param q2 True attitude quaternion component 2 - * @param q3 True attitude quaternion component 3 - * @param q4 True attitude quaternion component 4 - * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs - * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs - * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs - * @param xacc X acceleration m/s/s - * @param yacc Y acceleration m/s/s - * @param zacc Z acceleration m/s/s - * @param xgyro Angular speed around X axis rad/s - * @param ygyro Angular speed around Y axis rad/s - * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees - * @param lon Longitude in degrees - * @param alt Altitude in meters - * @param std_dev_horz Horizontal position standard deviation - * @param std_dev_vert Vertical position standard deviation - * @param vn True velocity in m/s in NORTH direction in earth-fixed NED frame - * @param ve True velocity in m/s in EAST direction in earth-fixed NED frame - * @param vd True velocity in m/s in DOWN direction in earth-fixed NED frame - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SIM_STATE_LEN]; - _mav_put_float(buf, 0, q1); - _mav_put_float(buf, 4, q2); - _mav_put_float(buf, 8, q3); - _mav_put_float(buf, 12, q4); - _mav_put_float(buf, 16, roll); - _mav_put_float(buf, 20, pitch); - _mav_put_float(buf, 24, yaw); - _mav_put_float(buf, 28, xacc); - _mav_put_float(buf, 32, yacc); - _mav_put_float(buf, 36, zacc); - _mav_put_float(buf, 40, xgyro); - _mav_put_float(buf, 44, ygyro); - _mav_put_float(buf, 48, zgyro); - _mav_put_float(buf, 52, lat); - _mav_put_float(buf, 56, lon); - _mav_put_float(buf, 60, alt); - _mav_put_float(buf, 64, std_dev_horz); - _mav_put_float(buf, 68, std_dev_vert); - _mav_put_float(buf, 72, vn); - _mav_put_float(buf, 76, ve); - _mav_put_float(buf, 80, vd); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIM_STATE_LEN); -#else - mavlink_sim_state_t packet; - packet.q1 = q1; - packet.q2 = q2; - packet.q3 = q3; - packet.q4 = q4; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.std_dev_horz = std_dev_horz; - packet.std_dev_vert = std_dev_vert; - packet.vn = vn; - packet.ve = ve; - packet.vd = vd; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIM_STATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SIM_STATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIM_STATE_LEN); -#endif -} - -/** - * @brief Pack a sim_state message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param q1 True attitude quaternion component 1 - * @param q2 True attitude quaternion component 2 - * @param q3 True attitude quaternion component 3 - * @param q4 True attitude quaternion component 4 - * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs - * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs - * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs - * @param xacc X acceleration m/s/s - * @param yacc Y acceleration m/s/s - * @param zacc Z acceleration m/s/s - * @param xgyro Angular speed around X axis rad/s - * @param ygyro Angular speed around Y axis rad/s - * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees - * @param lon Longitude in degrees - * @param alt Altitude in meters - * @param std_dev_horz Horizontal position standard deviation - * @param std_dev_vert Vertical position standard deviation - * @param vn True velocity in m/s in NORTH direction in earth-fixed NED frame - * @param ve True velocity in m/s in EAST direction in earth-fixed NED frame - * @param vd True velocity in m/s in DOWN direction in earth-fixed NED frame - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sim_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float q1,float q2,float q3,float q4,float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float lat,float lon,float alt,float std_dev_horz,float std_dev_vert,float vn,float ve,float vd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SIM_STATE_LEN]; - _mav_put_float(buf, 0, q1); - _mav_put_float(buf, 4, q2); - _mav_put_float(buf, 8, q3); - _mav_put_float(buf, 12, q4); - _mav_put_float(buf, 16, roll); - _mav_put_float(buf, 20, pitch); - _mav_put_float(buf, 24, yaw); - _mav_put_float(buf, 28, xacc); - _mav_put_float(buf, 32, yacc); - _mav_put_float(buf, 36, zacc); - _mav_put_float(buf, 40, xgyro); - _mav_put_float(buf, 44, ygyro); - _mav_put_float(buf, 48, zgyro); - _mav_put_float(buf, 52, lat); - _mav_put_float(buf, 56, lon); - _mav_put_float(buf, 60, alt); - _mav_put_float(buf, 64, std_dev_horz); - _mav_put_float(buf, 68, std_dev_vert); - _mav_put_float(buf, 72, vn); - _mav_put_float(buf, 76, ve); - _mav_put_float(buf, 80, vd); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIM_STATE_LEN); -#else - mavlink_sim_state_t packet; - packet.q1 = q1; - packet.q2 = q2; - packet.q3 = q3; - packet.q4 = q4; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.std_dev_horz = std_dev_horz; - packet.std_dev_vert = std_dev_vert; - packet.vn = vn; - packet.ve = ve; - packet.vd = vd; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIM_STATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SIM_STATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIM_STATE_LEN); -#endif -} - -/** - * @brief Encode a sim_state struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param sim_state C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sim_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state) -{ - return mavlink_msg_sim_state_pack(system_id, component_id, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd); -} - -/** - * @brief Encode a sim_state struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sim_state C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sim_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state) -{ - return mavlink_msg_sim_state_pack_chan(system_id, component_id, chan, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd); -} - -/** - * @brief Send a sim_state message - * @param chan MAVLink channel to send the message - * - * @param q1 True attitude quaternion component 1 - * @param q2 True attitude quaternion component 2 - * @param q3 True attitude quaternion component 3 - * @param q4 True attitude quaternion component 4 - * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs - * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs - * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs - * @param xacc X acceleration m/s/s - * @param yacc Y acceleration m/s/s - * @param zacc Z acceleration m/s/s - * @param xgyro Angular speed around X axis rad/s - * @param ygyro Angular speed around Y axis rad/s - * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees - * @param lon Longitude in degrees - * @param alt Altitude in meters - * @param std_dev_horz Horizontal position standard deviation - * @param std_dev_vert Vertical position standard deviation - * @param vn True velocity in m/s in NORTH direction in earth-fixed NED frame - * @param ve True velocity in m/s in EAST direction in earth-fixed NED frame - * @param vd True velocity in m/s in DOWN direction in earth-fixed NED frame - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SIM_STATE_LEN]; - _mav_put_float(buf, 0, q1); - _mav_put_float(buf, 4, q2); - _mav_put_float(buf, 8, q3); - _mav_put_float(buf, 12, q4); - _mav_put_float(buf, 16, roll); - _mav_put_float(buf, 20, pitch); - _mav_put_float(buf, 24, yaw); - _mav_put_float(buf, 28, xacc); - _mav_put_float(buf, 32, yacc); - _mav_put_float(buf, 36, zacc); - _mav_put_float(buf, 40, xgyro); - _mav_put_float(buf, 44, ygyro); - _mav_put_float(buf, 48, zgyro); - _mav_put_float(buf, 52, lat); - _mav_put_float(buf, 56, lon); - _mav_put_float(buf, 60, alt); - _mav_put_float(buf, 64, std_dev_horz); - _mav_put_float(buf, 68, std_dev_vert); - _mav_put_float(buf, 72, vn); - _mav_put_float(buf, 76, ve); - _mav_put_float(buf, 80, vd); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN); -#endif -#else - mavlink_sim_state_t packet; - packet.q1 = q1; - packet.q2 = q2; - packet.q3 = q3; - packet.q4 = q4; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.std_dev_horz = std_dev_horz; - packet.std_dev_vert = std_dev_vert; - packet.vn = vn; - packet.ve = ve; - packet.vd = vd; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)&packet, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)&packet, MAVLINK_MSG_ID_SIM_STATE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SIM_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_sim_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, q1); - _mav_put_float(buf, 4, q2); - _mav_put_float(buf, 8, q3); - _mav_put_float(buf, 12, q4); - _mav_put_float(buf, 16, roll); - _mav_put_float(buf, 20, pitch); - _mav_put_float(buf, 24, yaw); - _mav_put_float(buf, 28, xacc); - _mav_put_float(buf, 32, yacc); - _mav_put_float(buf, 36, zacc); - _mav_put_float(buf, 40, xgyro); - _mav_put_float(buf, 44, ygyro); - _mav_put_float(buf, 48, zgyro); - _mav_put_float(buf, 52, lat); - _mav_put_float(buf, 56, lon); - _mav_put_float(buf, 60, alt); - _mav_put_float(buf, 64, std_dev_horz); - _mav_put_float(buf, 68, std_dev_vert); - _mav_put_float(buf, 72, vn); - _mav_put_float(buf, 76, ve); - _mav_put_float(buf, 80, vd); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN); -#endif -#else - mavlink_sim_state_t *packet = (mavlink_sim_state_t *)msgbuf; - packet->q1 = q1; - packet->q2 = q2; - packet->q3 = q3; - packet->q4 = q4; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - packet->xacc = xacc; - packet->yacc = yacc; - packet->zacc = zacc; - packet->xgyro = xgyro; - packet->ygyro = ygyro; - packet->zgyro = zgyro; - packet->lat = lat; - packet->lon = lon; - packet->alt = alt; - packet->std_dev_horz = std_dev_horz; - packet->std_dev_vert = std_dev_vert; - packet->vn = vn; - packet->ve = ve; - packet->vd = vd; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SIM_STATE UNPACKING - - -/** - * @brief Get field q1 from sim_state message - * - * @return True attitude quaternion component 1 - */ -static inline float mavlink_msg_sim_state_get_q1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field q2 from sim_state message - * - * @return True attitude quaternion component 2 - */ -static inline float mavlink_msg_sim_state_get_q2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field q3 from sim_state message - * - * @return True attitude quaternion component 3 - */ -static inline float mavlink_msg_sim_state_get_q3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field q4 from sim_state message - * - * @return True attitude quaternion component 4 - */ -static inline float mavlink_msg_sim_state_get_q4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field roll from sim_state message - * - * @return Attitude roll expressed as Euler angles, not recommended except for human-readable outputs - */ -static inline float mavlink_msg_sim_state_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field pitch from sim_state message - * - * @return Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs - */ -static inline float mavlink_msg_sim_state_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field yaw from sim_state message - * - * @return Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs - */ -static inline float mavlink_msg_sim_state_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field xacc from sim_state message - * - * @return X acceleration m/s/s - */ -static inline float mavlink_msg_sim_state_get_xacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field yacc from sim_state message - * - * @return Y acceleration m/s/s - */ -static inline float mavlink_msg_sim_state_get_yacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field zacc from sim_state message - * - * @return Z acceleration m/s/s - */ -static inline float mavlink_msg_sim_state_get_zacc(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 36); -} - -/** - * @brief Get field xgyro from sim_state message - * - * @return Angular speed around X axis rad/s - */ -static inline float mavlink_msg_sim_state_get_xgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 40); -} - -/** - * @brief Get field ygyro from sim_state message - * - * @return Angular speed around Y axis rad/s - */ -static inline float mavlink_msg_sim_state_get_ygyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 44); -} - -/** - * @brief Get field zgyro from sim_state message - * - * @return Angular speed around Z axis rad/s - */ -static inline float mavlink_msg_sim_state_get_zgyro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 48); -} - -/** - * @brief Get field lat from sim_state message - * - * @return Latitude in degrees - */ -static inline float mavlink_msg_sim_state_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 52); -} - -/** - * @brief Get field lon from sim_state message - * - * @return Longitude in degrees - */ -static inline float mavlink_msg_sim_state_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 56); -} - -/** - * @brief Get field alt from sim_state message - * - * @return Altitude in meters - */ -static inline float mavlink_msg_sim_state_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 60); -} - -/** - * @brief Get field std_dev_horz from sim_state message - * - * @return Horizontal position standard deviation - */ -static inline float mavlink_msg_sim_state_get_std_dev_horz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 64); -} - -/** - * @brief Get field std_dev_vert from sim_state message - * - * @return Vertical position standard deviation - */ -static inline float mavlink_msg_sim_state_get_std_dev_vert(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 68); -} - -/** - * @brief Get field vn from sim_state message - * - * @return True velocity in m/s in NORTH direction in earth-fixed NED frame - */ -static inline float mavlink_msg_sim_state_get_vn(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 72); -} - -/** - * @brief Get field ve from sim_state message - * - * @return True velocity in m/s in EAST direction in earth-fixed NED frame - */ -static inline float mavlink_msg_sim_state_get_ve(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 76); -} - -/** - * @brief Get field vd from sim_state message - * - * @return True velocity in m/s in DOWN direction in earth-fixed NED frame - */ -static inline float mavlink_msg_sim_state_get_vd(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 80); -} - -/** - * @brief Decode a sim_state message into a struct - * - * @param msg The message to decode - * @param sim_state C-struct to decode the message contents into - */ -static inline void mavlink_msg_sim_state_decode(const mavlink_message_t* msg, mavlink_sim_state_t* sim_state) -{ -#if MAVLINK_NEED_BYTE_SWAP - sim_state->q1 = mavlink_msg_sim_state_get_q1(msg); - sim_state->q2 = mavlink_msg_sim_state_get_q2(msg); - sim_state->q3 = mavlink_msg_sim_state_get_q3(msg); - sim_state->q4 = mavlink_msg_sim_state_get_q4(msg); - sim_state->roll = mavlink_msg_sim_state_get_roll(msg); - sim_state->pitch = mavlink_msg_sim_state_get_pitch(msg); - sim_state->yaw = mavlink_msg_sim_state_get_yaw(msg); - sim_state->xacc = mavlink_msg_sim_state_get_xacc(msg); - sim_state->yacc = mavlink_msg_sim_state_get_yacc(msg); - sim_state->zacc = mavlink_msg_sim_state_get_zacc(msg); - sim_state->xgyro = mavlink_msg_sim_state_get_xgyro(msg); - sim_state->ygyro = mavlink_msg_sim_state_get_ygyro(msg); - sim_state->zgyro = mavlink_msg_sim_state_get_zgyro(msg); - sim_state->lat = mavlink_msg_sim_state_get_lat(msg); - sim_state->lon = mavlink_msg_sim_state_get_lon(msg); - sim_state->alt = mavlink_msg_sim_state_get_alt(msg); - sim_state->std_dev_horz = mavlink_msg_sim_state_get_std_dev_horz(msg); - sim_state->std_dev_vert = mavlink_msg_sim_state_get_std_dev_vert(msg); - sim_state->vn = mavlink_msg_sim_state_get_vn(msg); - sim_state->ve = mavlink_msg_sim_state_get_ve(msg); - sim_state->vd = mavlink_msg_sim_state_get_vd(msg); -#else - memcpy(sim_state, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SIM_STATE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h deleted file mode 100644 index be2629dd8..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h +++ /dev/null @@ -1,401 +0,0 @@ -// MESSAGE STATE_CORRECTION PACKING - -#define MAVLINK_MSG_ID_STATE_CORRECTION 64 - -typedef struct __mavlink_state_correction_t -{ - float xErr; ///< x position error - float yErr; ///< y position error - float zErr; ///< z position error - float rollErr; ///< roll error (radians) - float pitchErr; ///< pitch error (radians) - float yawErr; ///< yaw error (radians) - float vxErr; ///< x velocity - float vyErr; ///< y velocity - float vzErr; ///< z velocity -} mavlink_state_correction_t; - -#define MAVLINK_MSG_ID_STATE_CORRECTION_LEN 36 -#define MAVLINK_MSG_ID_64_LEN 36 - -#define MAVLINK_MSG_ID_STATE_CORRECTION_CRC 130 -#define MAVLINK_MSG_ID_64_CRC 130 - - - -#define MAVLINK_MESSAGE_INFO_STATE_CORRECTION { \ - "STATE_CORRECTION", \ - 9, \ - { { "xErr", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_state_correction_t, xErr) }, \ - { "yErr", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_state_correction_t, yErr) }, \ - { "zErr", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_state_correction_t, zErr) }, \ - { "rollErr", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_state_correction_t, rollErr) }, \ - { "pitchErr", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_state_correction_t, pitchErr) }, \ - { "yawErr", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_state_correction_t, yawErr) }, \ - { "vxErr", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_state_correction_t, vxErr) }, \ - { "vyErr", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_state_correction_t, vyErr) }, \ - { "vzErr", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_state_correction_t, vzErr) }, \ - } \ -} - - -/** - * @brief Pack a state_correction message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param xErr x position error - * @param yErr y position error - * @param zErr z position error - * @param rollErr roll error (radians) - * @param pitchErr pitch error (radians) - * @param yawErr yaw error (radians) - * @param vxErr x velocity - * @param vyErr y velocity - * @param vzErr z velocity - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_STATE_CORRECTION_LEN]; - _mav_put_float(buf, 0, xErr); - _mav_put_float(buf, 4, yErr); - _mav_put_float(buf, 8, zErr); - _mav_put_float(buf, 12, rollErr); - _mav_put_float(buf, 16, pitchErr); - _mav_put_float(buf, 20, yawErr); - _mav_put_float(buf, 24, vxErr); - _mav_put_float(buf, 28, vyErr); - _mav_put_float(buf, 32, vzErr); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#else - mavlink_state_correction_t packet; - packet.xErr = xErr; - packet.yErr = yErr; - packet.zErr = zErr; - packet.rollErr = rollErr; - packet.pitchErr = pitchErr; - packet.yawErr = yawErr; - packet.vxErr = vxErr; - packet.vyErr = vyErr; - packet.vzErr = vzErr; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif -} - -/** - * @brief Pack a state_correction message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param xErr x position error - * @param yErr y position error - * @param zErr z position error - * @param rollErr roll error (radians) - * @param pitchErr pitch error (radians) - * @param yawErr yaw error (radians) - * @param vxErr x velocity - * @param vyErr y velocity - * @param vzErr z velocity - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float xErr,float yErr,float zErr,float rollErr,float pitchErr,float yawErr,float vxErr,float vyErr,float vzErr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_STATE_CORRECTION_LEN]; - _mav_put_float(buf, 0, xErr); - _mav_put_float(buf, 4, yErr); - _mav_put_float(buf, 8, zErr); - _mav_put_float(buf, 12, rollErr); - _mav_put_float(buf, 16, pitchErr); - _mav_put_float(buf, 20, yawErr); - _mav_put_float(buf, 24, vxErr); - _mav_put_float(buf, 28, vyErr); - _mav_put_float(buf, 32, vzErr); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#else - mavlink_state_correction_t packet; - packet.xErr = xErr; - packet.yErr = yErr; - packet.zErr = zErr; - packet.rollErr = rollErr; - packet.pitchErr = pitchErr; - packet.yawErr = yawErr; - packet.vxErr = vxErr; - packet.vyErr = vyErr; - packet.vzErr = vzErr; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif -} - -/** - * @brief Encode a state_correction struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param state_correction C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_state_correction_t* state_correction) -{ - return mavlink_msg_state_correction_pack(system_id, component_id, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr); -} - -/** - * @brief Encode a state_correction struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param state_correction C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_state_correction_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_state_correction_t* state_correction) -{ - return mavlink_msg_state_correction_pack_chan(system_id, component_id, chan, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr); -} - -/** - * @brief Send a state_correction message - * @param chan MAVLink channel to send the message - * - * @param xErr x position error - * @param yErr y position error - * @param zErr z position error - * @param rollErr roll error (radians) - * @param pitchErr pitch error (radians) - * @param yawErr yaw error (radians) - * @param vxErr x velocity - * @param vyErr y velocity - * @param vzErr z velocity - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_STATE_CORRECTION_LEN]; - _mav_put_float(buf, 0, xErr); - _mav_put_float(buf, 4, yErr); - _mav_put_float(buf, 8, zErr); - _mav_put_float(buf, 12, rollErr); - _mav_put_float(buf, 16, pitchErr); - _mav_put_float(buf, 20, yawErr); - _mav_put_float(buf, 24, vxErr); - _mav_put_float(buf, 28, vyErr); - _mav_put_float(buf, 32, vzErr); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif -#else - mavlink_state_correction_t packet; - packet.xErr = xErr; - packet.yErr = yErr; - packet.zErr = zErr; - packet.rollErr = rollErr; - packet.pitchErr = pitchErr; - packet.yawErr = yawErr; - packet.vxErr = vxErr; - packet.vyErr = vyErr; - packet.vzErr = vzErr; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)&packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)&packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_STATE_CORRECTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_state_correction_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, xErr); - _mav_put_float(buf, 4, yErr); - _mav_put_float(buf, 8, zErr); - _mav_put_float(buf, 12, rollErr); - _mav_put_float(buf, 16, pitchErr); - _mav_put_float(buf, 20, yawErr); - _mav_put_float(buf, 24, vxErr); - _mav_put_float(buf, 28, vyErr); - _mav_put_float(buf, 32, vzErr); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif -#else - mavlink_state_correction_t *packet = (mavlink_state_correction_t *)msgbuf; - packet->xErr = xErr; - packet->yErr = yErr; - packet->zErr = zErr; - packet->rollErr = rollErr; - packet->pitchErr = pitchErr; - packet->yawErr = yawErr; - packet->vxErr = vxErr; - packet->vyErr = vyErr; - packet->vzErr = vzErr; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE STATE_CORRECTION UNPACKING - - -/** - * @brief Get field xErr from state_correction message - * - * @return x position error - */ -static inline float mavlink_msg_state_correction_get_xErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field yErr from state_correction message - * - * @return y position error - */ -static inline float mavlink_msg_state_correction_get_yErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field zErr from state_correction message - * - * @return z position error - */ -static inline float mavlink_msg_state_correction_get_zErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field rollErr from state_correction message - * - * @return roll error (radians) - */ -static inline float mavlink_msg_state_correction_get_rollErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field pitchErr from state_correction message - * - * @return pitch error (radians) - */ -static inline float mavlink_msg_state_correction_get_pitchErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field yawErr from state_correction message - * - * @return yaw error (radians) - */ -static inline float mavlink_msg_state_correction_get_yawErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field vxErr from state_correction message - * - * @return x velocity - */ -static inline float mavlink_msg_state_correction_get_vxErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field vyErr from state_correction message - * - * @return y velocity - */ -static inline float mavlink_msg_state_correction_get_vyErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field vzErr from state_correction message - * - * @return z velocity - */ -static inline float mavlink_msg_state_correction_get_vzErr(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Decode a state_correction message into a struct - * - * @param msg The message to decode - * @param state_correction C-struct to decode the message contents into - */ -static inline void mavlink_msg_state_correction_decode(const mavlink_message_t* msg, mavlink_state_correction_t* state_correction) -{ -#if MAVLINK_NEED_BYTE_SWAP - state_correction->xErr = mavlink_msg_state_correction_get_xErr(msg); - state_correction->yErr = mavlink_msg_state_correction_get_yErr(msg); - state_correction->zErr = mavlink_msg_state_correction_get_zErr(msg); - state_correction->rollErr = mavlink_msg_state_correction_get_rollErr(msg); - state_correction->pitchErr = mavlink_msg_state_correction_get_pitchErr(msg); - state_correction->yawErr = mavlink_msg_state_correction_get_yawErr(msg); - state_correction->vxErr = mavlink_msg_state_correction_get_vxErr(msg); - state_correction->vyErr = mavlink_msg_state_correction_get_vyErr(msg); - state_correction->vzErr = mavlink_msg_state_correction_get_vzErr(msg); -#else - memcpy(state_correction, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_STATE_CORRECTION_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h deleted file mode 100644 index c8c2547b3..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h +++ /dev/null @@ -1,225 +0,0 @@ -// MESSAGE STATUSTEXT PACKING - -#define MAVLINK_MSG_ID_STATUSTEXT 253 - -typedef struct __mavlink_statustext_t -{ - uint8_t severity; ///< Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. - char text[50]; ///< Status text message, without null termination character -} mavlink_statustext_t; - -#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 -#define MAVLINK_MSG_ID_253_LEN 51 - -#define MAVLINK_MSG_ID_STATUSTEXT_CRC 83 -#define MAVLINK_MSG_ID_253_CRC 83 - -#define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 - -#define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ - "STATUSTEXT", \ - 2, \ - { { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ - { "text", NULL, MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_statustext_t, text) }, \ - } \ -} - - -/** - * @brief Pack a statustext message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. - * @param text Status text message, without null termination character - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t severity, const char *text) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; - _mav_put_uint8_t(buf, 0, severity); - _mav_put_char_array(buf, 1, text, 50); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#else - mavlink_statustext_t packet; - packet.severity = severity; - mav_array_memcpy(packet.text, text, sizeof(char)*50); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif -} - -/** - * @brief Pack a statustext message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. - * @param text Status text message, without null termination character - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t severity,const char *text) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; - _mav_put_uint8_t(buf, 0, severity); - _mav_put_char_array(buf, 1, text, 50); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#else - mavlink_statustext_t packet; - packet.severity = severity; - mav_array_memcpy(packet.text, text, sizeof(char)*50); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif -} - -/** - * @brief Encode a statustext struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param statustext C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_statustext_t* statustext) -{ - return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text); -} - -/** - * @brief Encode a statustext struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param statustext C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_statustext_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_statustext_t* statustext) -{ - return mavlink_msg_statustext_pack_chan(system_id, component_id, chan, msg, statustext->severity, statustext->text); -} - -/** - * @brief Send a statustext message - * @param chan MAVLink channel to send the message - * - * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. - * @param text Status text message, without null termination character - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; - _mav_put_uint8_t(buf, 0, severity); - _mav_put_char_array(buf, 1, text, 50); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif -#else - mavlink_statustext_t packet; - packet.severity = severity; - mav_array_memcpy(packet.text, text, sizeof(char)*50); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_STATUSTEXT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t severity, const char *text) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, severity); - _mav_put_char_array(buf, 1, text, 50); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif -#else - mavlink_statustext_t *packet = (mavlink_statustext_t *)msgbuf; - packet->severity = severity; - mav_array_memcpy(packet->text, text, sizeof(char)*50); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE STATUSTEXT UNPACKING - - -/** - * @brief Get field severity from statustext message - * - * @return Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. - */ -static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field text from statustext message - * - * @return Status text message, without null termination character - */ -static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, char *text) -{ - return _MAV_RETURN_char_array(msg, text, 50, 1); -} - -/** - * @brief Decode a statustext message into a struct - * - * @param msg The message to decode - * @param statustext C-struct to decode the message contents into - */ -static inline void mavlink_msg_statustext_decode(const mavlink_message_t* msg, mavlink_statustext_t* statustext) -{ -#if MAVLINK_NEED_BYTE_SWAP - statustext->severity = mavlink_msg_statustext_get_severity(msg); - mavlink_msg_statustext_get_text(msg, statustext->text); -#else - memcpy(statustext, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h deleted file mode 100644 index 1b1730d5f..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h +++ /dev/null @@ -1,497 +0,0 @@ -// MESSAGE SYS_STATUS PACKING - -#define MAVLINK_MSG_ID_SYS_STATUS 1 - -typedef struct __mavlink_sys_status_t -{ - uint32_t onboard_control_sensors_present; ///< Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - uint32_t onboard_control_sensors_enabled; ///< Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - uint32_t onboard_control_sensors_health; ///< Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - uint16_t voltage_battery; ///< Battery voltage, in millivolts (1 = 1 millivolt) - int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - uint16_t drop_rate_comm; ///< Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - uint16_t errors_comm; ///< Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - uint16_t errors_count1; ///< Autopilot-specific errors - uint16_t errors_count2; ///< Autopilot-specific errors - uint16_t errors_count3; ///< Autopilot-specific errors - uint16_t errors_count4; ///< Autopilot-specific errors - int8_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery -} mavlink_sys_status_t; - -#define MAVLINK_MSG_ID_SYS_STATUS_LEN 31 -#define MAVLINK_MSG_ID_1_LEN 31 - -#define MAVLINK_MSG_ID_SYS_STATUS_CRC 124 -#define MAVLINK_MSG_ID_1_CRC 124 - - - -#define MAVLINK_MESSAGE_INFO_SYS_STATUS { \ - "SYS_STATUS", \ - 13, \ - { { "onboard_control_sensors_present", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_sys_status_t, onboard_control_sensors_present) }, \ - { "onboard_control_sensors_enabled", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled) }, \ - { "onboard_control_sensors_health", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_sys_status_t, onboard_control_sensors_health) }, \ - { "load", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_sys_status_t, load) }, \ - { "voltage_battery", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_sys_status_t, voltage_battery) }, \ - { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_sys_status_t, current_battery) }, \ - { "drop_rate_comm", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_sys_status_t, drop_rate_comm) }, \ - { "errors_comm", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_sys_status_t, errors_comm) }, \ - { "errors_count1", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_sys_status_t, errors_count1) }, \ - { "errors_count2", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_sys_status_t, errors_count2) }, \ - { "errors_count3", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_sys_status_t, errors_count3) }, \ - { "errors_count4", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_sys_status_t, errors_count4) }, \ - { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 30, offsetof(mavlink_sys_status_t, battery_remaining) }, \ - } \ -} - - -/** - * @brief Pack a sys_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) - * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery - * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - * @param errors_count1 Autopilot-specific errors - * @param errors_count2 Autopilot-specific errors - * @param errors_count3 Autopilot-specific errors - * @param errors_count4 Autopilot-specific errors - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); - _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); - _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); - _mav_put_uint16_t(buf, 12, load); - _mav_put_uint16_t(buf, 14, voltage_battery); - _mav_put_int16_t(buf, 16, current_battery); - _mav_put_uint16_t(buf, 18, drop_rate_comm); - _mav_put_uint16_t(buf, 20, errors_comm); - _mav_put_uint16_t(buf, 22, errors_count1); - _mav_put_uint16_t(buf, 24, errors_count2); - _mav_put_uint16_t(buf, 26, errors_count3); - _mav_put_uint16_t(buf, 28, errors_count4); - _mav_put_int8_t(buf, 30, battery_remaining); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); -#else - mavlink_sys_status_t packet; - packet.onboard_control_sensors_present = onboard_control_sensors_present; - packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled; - packet.onboard_control_sensors_health = onboard_control_sensors_health; - packet.load = load; - packet.voltage_battery = voltage_battery; - packet.current_battery = current_battery; - packet.drop_rate_comm = drop_rate_comm; - packet.errors_comm = errors_comm; - packet.errors_count1 = errors_count1; - packet.errors_count2 = errors_count2; - packet.errors_count3 = errors_count3; - packet.errors_count4 = errors_count4; - packet.battery_remaining = battery_remaining; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_LEN); -#endif -} - -/** - * @brief Pack a sys_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) - * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery - * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - * @param errors_count1 Autopilot-specific errors - * @param errors_count2 Autopilot-specific errors - * @param errors_count3 Autopilot-specific errors - * @param errors_count4 Autopilot-specific errors - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t onboard_control_sensors_present,uint32_t onboard_control_sensors_enabled,uint32_t onboard_control_sensors_health,uint16_t load,uint16_t voltage_battery,int16_t current_battery,int8_t battery_remaining,uint16_t drop_rate_comm,uint16_t errors_comm,uint16_t errors_count1,uint16_t errors_count2,uint16_t errors_count3,uint16_t errors_count4) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); - _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); - _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); - _mav_put_uint16_t(buf, 12, load); - _mav_put_uint16_t(buf, 14, voltage_battery); - _mav_put_int16_t(buf, 16, current_battery); - _mav_put_uint16_t(buf, 18, drop_rate_comm); - _mav_put_uint16_t(buf, 20, errors_comm); - _mav_put_uint16_t(buf, 22, errors_count1); - _mav_put_uint16_t(buf, 24, errors_count2); - _mav_put_uint16_t(buf, 26, errors_count3); - _mav_put_uint16_t(buf, 28, errors_count4); - _mav_put_int8_t(buf, 30, battery_remaining); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); -#else - mavlink_sys_status_t packet; - packet.onboard_control_sensors_present = onboard_control_sensors_present; - packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled; - packet.onboard_control_sensors_health = onboard_control_sensors_health; - packet.load = load; - packet.voltage_battery = voltage_battery; - packet.current_battery = current_battery; - packet.drop_rate_comm = drop_rate_comm; - packet.errors_comm = errors_comm; - packet.errors_count1 = errors_count1; - packet.errors_count2 = errors_count2; - packet.errors_count3 = errors_count3; - packet.errors_count4 = errors_count4; - packet.battery_remaining = battery_remaining; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN); -#endif -} - -/** - * @brief Encode a sys_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param sys_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) -{ - return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); -} - -/** - * @brief Encode a sys_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sys_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sys_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) -{ - return mavlink_msg_sys_status_pack_chan(system_id, component_id, chan, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); -} - -/** - * @brief Send a sys_status message - * @param chan MAVLink channel to send the message - * - * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) - * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery - * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - * @param errors_count1 Autopilot-specific errors - * @param errors_count2 Autopilot-specific errors - * @param errors_count3 Autopilot-specific errors - * @param errors_count4 Autopilot-specific errors - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN]; - _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); - _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); - _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); - _mav_put_uint16_t(buf, 12, load); - _mav_put_uint16_t(buf, 14, voltage_battery); - _mav_put_int16_t(buf, 16, current_battery); - _mav_put_uint16_t(buf, 18, drop_rate_comm); - _mav_put_uint16_t(buf, 20, errors_comm); - _mav_put_uint16_t(buf, 22, errors_count1); - _mav_put_uint16_t(buf, 24, errors_count2); - _mav_put_uint16_t(buf, 26, errors_count3); - _mav_put_uint16_t(buf, 28, errors_count4); - _mav_put_int8_t(buf, 30, battery_remaining); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); -#endif -#else - mavlink_sys_status_t packet; - packet.onboard_control_sensors_present = onboard_control_sensors_present; - packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled; - packet.onboard_control_sensors_health = onboard_control_sensors_health; - packet.load = load; - packet.voltage_battery = voltage_battery; - packet.current_battery = current_battery; - packet.drop_rate_comm = drop_rate_comm; - packet.errors_comm = errors_comm; - packet.errors_count1 = errors_count1; - packet.errors_count2 = errors_count2; - packet.errors_count3 = errors_count3; - packet.errors_count4 = errors_count4; - packet.battery_remaining = battery_remaining; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SYS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_sys_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); - _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); - _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); - _mav_put_uint16_t(buf, 12, load); - _mav_put_uint16_t(buf, 14, voltage_battery); - _mav_put_int16_t(buf, 16, current_battery); - _mav_put_uint16_t(buf, 18, drop_rate_comm); - _mav_put_uint16_t(buf, 20, errors_comm); - _mav_put_uint16_t(buf, 22, errors_count1); - _mav_put_uint16_t(buf, 24, errors_count2); - _mav_put_uint16_t(buf, 26, errors_count3); - _mav_put_uint16_t(buf, 28, errors_count4); - _mav_put_int8_t(buf, 30, battery_remaining); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); -#endif -#else - mavlink_sys_status_t *packet = (mavlink_sys_status_t *)msgbuf; - packet->onboard_control_sensors_present = onboard_control_sensors_present; - packet->onboard_control_sensors_enabled = onboard_control_sensors_enabled; - packet->onboard_control_sensors_health = onboard_control_sensors_health; - packet->load = load; - packet->voltage_battery = voltage_battery; - packet->current_battery = current_battery; - packet->drop_rate_comm = drop_rate_comm; - packet->errors_comm = errors_comm; - packet->errors_count1 = errors_count1; - packet->errors_count2 = errors_count2; - packet->errors_count3 = errors_count3; - packet->errors_count4 = errors_count4; - packet->battery_remaining = battery_remaining; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SYS_STATUS UNPACKING - - -/** - * @brief Get field onboard_control_sensors_present from sys_status message - * - * @return Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - */ -static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_present(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field onboard_control_sensors_enabled from sys_status message - * - * @return Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - */ -static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enabled(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 4); -} - -/** - * @brief Get field onboard_control_sensors_health from sys_status message - * - * @return Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR - */ -static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_health(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 8); -} - -/** - * @brief Get field load from sys_status message - * - * @return Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - */ -static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field voltage_battery from sys_status message - * - * @return Battery voltage, in millivolts (1 = 1 millivolt) - */ -static inline uint16_t mavlink_msg_sys_status_get_voltage_battery(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 14); -} - -/** - * @brief Get field current_battery from sys_status message - * - * @return Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - */ -static inline int16_t mavlink_msg_sys_status_get_current_battery(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 16); -} - -/** - * @brief Get field battery_remaining from sys_status message - * - * @return Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery - */ -static inline int8_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int8_t(msg, 30); -} - -/** - * @brief Get field drop_rate_comm from sys_status message - * - * @return Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - */ -static inline uint16_t mavlink_msg_sys_status_get_drop_rate_comm(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 18); -} - -/** - * @brief Get field errors_comm from sys_status message - * - * @return Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - */ -static inline uint16_t mavlink_msg_sys_status_get_errors_comm(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 20); -} - -/** - * @brief Get field errors_count1 from sys_status message - * - * @return Autopilot-specific errors - */ -static inline uint16_t mavlink_msg_sys_status_get_errors_count1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 22); -} - -/** - * @brief Get field errors_count2 from sys_status message - * - * @return Autopilot-specific errors - */ -static inline uint16_t mavlink_msg_sys_status_get_errors_count2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 24); -} - -/** - * @brief Get field errors_count3 from sys_status message - * - * @return Autopilot-specific errors - */ -static inline uint16_t mavlink_msg_sys_status_get_errors_count3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 26); -} - -/** - * @brief Get field errors_count4 from sys_status message - * - * @return Autopilot-specific errors - */ -static inline uint16_t mavlink_msg_sys_status_get_errors_count4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 28); -} - -/** - * @brief Decode a sys_status message into a struct - * - * @param msg The message to decode - * @param sys_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - sys_status->onboard_control_sensors_present = mavlink_msg_sys_status_get_onboard_control_sensors_present(msg); - sys_status->onboard_control_sensors_enabled = mavlink_msg_sys_status_get_onboard_control_sensors_enabled(msg); - sys_status->onboard_control_sensors_health = mavlink_msg_sys_status_get_onboard_control_sensors_health(msg); - sys_status->load = mavlink_msg_sys_status_get_load(msg); - sys_status->voltage_battery = mavlink_msg_sys_status_get_voltage_battery(msg); - sys_status->current_battery = mavlink_msg_sys_status_get_current_battery(msg); - sys_status->drop_rate_comm = mavlink_msg_sys_status_get_drop_rate_comm(msg); - sys_status->errors_comm = mavlink_msg_sys_status_get_errors_comm(msg); - sys_status->errors_count1 = mavlink_msg_sys_status_get_errors_count1(msg); - sys_status->errors_count2 = mavlink_msg_sys_status_get_errors_count2(msg); - sys_status->errors_count3 = mavlink_msg_sys_status_get_errors_count3(msg); - sys_status->errors_count4 = mavlink_msg_sys_status_get_errors_count4(msg); - sys_status->battery_remaining = mavlink_msg_sys_status_get_battery_remaining(msg); -#else - memcpy(sys_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SYS_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h deleted file mode 100644 index 988c669c3..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE SYSTEM_TIME PACKING - -#define MAVLINK_MSG_ID_SYSTEM_TIME 2 - -typedef struct __mavlink_system_time_t -{ - uint64_t time_unix_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. - uint32_t time_boot_ms; ///< Timestamp of the component clock since boot time in milliseconds. -} mavlink_system_time_t; - -#define MAVLINK_MSG_ID_SYSTEM_TIME_LEN 12 -#define MAVLINK_MSG_ID_2_LEN 12 - -#define MAVLINK_MSG_ID_SYSTEM_TIME_CRC 137 -#define MAVLINK_MSG_ID_2_CRC 137 - - - -#define MAVLINK_MESSAGE_INFO_SYSTEM_TIME { \ - "SYSTEM_TIME", \ - 2, \ - { { "time_unix_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_system_time_t, time_unix_usec) }, \ - { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_system_time_t, time_boot_ms) }, \ - } \ -} - - -/** - * @brief Pack a system_time message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch. - * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_unix_usec, uint32_t time_boot_ms) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SYSTEM_TIME_LEN]; - _mav_put_uint64_t(buf, 0, time_unix_usec); - _mav_put_uint32_t(buf, 8, time_boot_ms); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); -#else - mavlink_system_time_t packet; - packet.time_unix_usec = time_unix_usec; - packet.time_boot_ms = time_boot_ms; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); -#endif -} - -/** - * @brief Pack a system_time message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch. - * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_unix_usec,uint32_t time_boot_ms) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SYSTEM_TIME_LEN]; - _mav_put_uint64_t(buf, 0, time_unix_usec); - _mav_put_uint32_t(buf, 8, time_boot_ms); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); -#else - mavlink_system_time_t packet; - packet.time_unix_usec = time_unix_usec; - packet.time_boot_ms = time_boot_ms; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); -#endif -} - -/** - * @brief Encode a system_time struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param system_time C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_system_time_t* system_time) -{ - return mavlink_msg_system_time_pack(system_id, component_id, msg, system_time->time_unix_usec, system_time->time_boot_ms); -} - -/** - * @brief Encode a system_time struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param system_time C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_system_time_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_system_time_t* system_time) -{ - return mavlink_msg_system_time_pack_chan(system_id, component_id, chan, msg, system_time->time_unix_usec, system_time->time_boot_ms); -} - -/** - * @brief Send a system_time message - * @param chan MAVLink channel to send the message - * - * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch. - * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_unix_usec, uint32_t time_boot_ms) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SYSTEM_TIME_LEN]; - _mav_put_uint64_t(buf, 0, time_unix_usec); - _mav_put_uint32_t(buf, 8, time_boot_ms); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); -#endif -#else - mavlink_system_time_t packet; - packet.time_unix_usec = time_unix_usec; - packet.time_boot_ms = time_boot_ms; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)&packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)&packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SYSTEM_TIME_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_system_time_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_unix_usec, uint32_t time_boot_ms) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_unix_usec); - _mav_put_uint32_t(buf, 8, time_boot_ms); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); -#endif -#else - mavlink_system_time_t *packet = (mavlink_system_time_t *)msgbuf; - packet->time_unix_usec = time_unix_usec; - packet->time_boot_ms = time_boot_ms; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SYSTEM_TIME UNPACKING - - -/** - * @brief Get field time_unix_usec from system_time message - * - * @return Timestamp of the master clock in microseconds since UNIX epoch. - */ -static inline uint64_t mavlink_msg_system_time_get_time_unix_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field time_boot_ms from system_time message - * - * @return Timestamp of the component clock since boot time in milliseconds. - */ -static inline uint32_t mavlink_msg_system_time_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 8); -} - -/** - * @brief Decode a system_time message into a struct - * - * @param msg The message to decode - * @param system_time C-struct to decode the message contents into - */ -static inline void mavlink_msg_system_time_decode(const mavlink_message_t* msg, mavlink_system_time_t* system_time) -{ -#if MAVLINK_NEED_BYTE_SWAP - system_time->time_unix_usec = mavlink_msg_system_time_get_time_unix_usec(msg); - system_time->time_boot_ms = mavlink_msg_system_time_get_time_boot_ms(msg); -#else - memcpy(system_time, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SYSTEM_TIME_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h deleted file mode 100644 index b130ee50b..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE VFR_HUD PACKING - -#define MAVLINK_MSG_ID_VFR_HUD 74 - -typedef struct __mavlink_vfr_hud_t -{ - float airspeed; ///< Current airspeed in m/s - float groundspeed; ///< Current ground speed in m/s - float alt; ///< Current altitude (MSL), in meters - float climb; ///< Current climb rate in meters/second - int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north) - uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100 -} mavlink_vfr_hud_t; - -#define MAVLINK_MSG_ID_VFR_HUD_LEN 20 -#define MAVLINK_MSG_ID_74_LEN 20 - -#define MAVLINK_MSG_ID_VFR_HUD_CRC 20 -#define MAVLINK_MSG_ID_74_CRC 20 - - - -#define MAVLINK_MESSAGE_INFO_VFR_HUD { \ - "VFR_HUD", \ - 6, \ - { { "airspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_vfr_hud_t, airspeed) }, \ - { "groundspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_vfr_hud_t, groundspeed) }, \ - { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vfr_hud_t, alt) }, \ - { "climb", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vfr_hud_t, climb) }, \ - { "heading", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_vfr_hud_t, heading) }, \ - { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_vfr_hud_t, throttle) }, \ - } \ -} - - -/** - * @brief Pack a vfr_hud message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param airspeed Current airspeed in m/s - * @param groundspeed Current ground speed in m/s - * @param heading Current heading in degrees, in compass units (0..360, 0=north) - * @param throttle Current throttle setting in integer percent, 0 to 100 - * @param alt Current altitude (MSL), in meters - * @param climb Current climb rate in meters/second - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_VFR_HUD_LEN]; - _mav_put_float(buf, 0, airspeed); - _mav_put_float(buf, 4, groundspeed); - _mav_put_float(buf, 8, alt); - _mav_put_float(buf, 12, climb); - _mav_put_int16_t(buf, 16, heading); - _mav_put_uint16_t(buf, 18, throttle); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VFR_HUD_LEN); -#else - mavlink_vfr_hud_t packet; - packet.airspeed = airspeed; - packet.groundspeed = groundspeed; - packet.alt = alt; - packet.climb = climb; - packet.heading = heading; - packet.throttle = throttle; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VFR_HUD_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_VFR_HUD; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VFR_HUD_LEN); -#endif -} - -/** - * @brief Pack a vfr_hud message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param airspeed Current airspeed in m/s - * @param groundspeed Current ground speed in m/s - * @param heading Current heading in degrees, in compass units (0..360, 0=north) - * @param throttle Current throttle setting in integer percent, 0 to 100 - * @param alt Current altitude (MSL), in meters - * @param climb Current climb rate in meters/second - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float airspeed,float groundspeed,int16_t heading,uint16_t throttle,float alt,float climb) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_VFR_HUD_LEN]; - _mav_put_float(buf, 0, airspeed); - _mav_put_float(buf, 4, groundspeed); - _mav_put_float(buf, 8, alt); - _mav_put_float(buf, 12, climb); - _mav_put_int16_t(buf, 16, heading); - _mav_put_uint16_t(buf, 18, throttle); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VFR_HUD_LEN); -#else - mavlink_vfr_hud_t packet; - packet.airspeed = airspeed; - packet.groundspeed = groundspeed; - packet.alt = alt; - packet.climb = climb; - packet.heading = heading; - packet.throttle = throttle; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VFR_HUD_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_VFR_HUD; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN); -#endif -} - -/** - * @brief Encode a vfr_hud struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param vfr_hud C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vfr_hud_t* vfr_hud) -{ - return mavlink_msg_vfr_hud_pack(system_id, component_id, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); -} - -/** - * @brief Encode a vfr_hud struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param vfr_hud C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vfr_hud_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vfr_hud_t* vfr_hud) -{ - return mavlink_msg_vfr_hud_pack_chan(system_id, component_id, chan, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); -} - -/** - * @brief Send a vfr_hud message - * @param chan MAVLink channel to send the message - * - * @param airspeed Current airspeed in m/s - * @param groundspeed Current ground speed in m/s - * @param heading Current heading in degrees, in compass units (0..360, 0=north) - * @param throttle Current throttle setting in integer percent, 0 to 100 - * @param alt Current altitude (MSL), in meters - * @param climb Current climb rate in meters/second - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_VFR_HUD_LEN]; - _mav_put_float(buf, 0, airspeed); - _mav_put_float(buf, 4, groundspeed); - _mav_put_float(buf, 8, alt); - _mav_put_float(buf, 12, climb); - _mav_put_int16_t(buf, 16, heading); - _mav_put_uint16_t(buf, 18, throttle); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN); -#endif -#else - mavlink_vfr_hud_t packet; - packet.airspeed = airspeed; - packet.groundspeed = groundspeed; - packet.alt = alt; - packet.climb = climb; - packet.heading = heading; - packet.throttle = throttle; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)&packet, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)&packet, MAVLINK_MSG_ID_VFR_HUD_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_VFR_HUD_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_vfr_hud_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, airspeed); - _mav_put_float(buf, 4, groundspeed); - _mav_put_float(buf, 8, alt); - _mav_put_float(buf, 12, climb); - _mav_put_int16_t(buf, 16, heading); - _mav_put_uint16_t(buf, 18, throttle); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN); -#endif -#else - mavlink_vfr_hud_t *packet = (mavlink_vfr_hud_t *)msgbuf; - packet->airspeed = airspeed; - packet->groundspeed = groundspeed; - packet->alt = alt; - packet->climb = climb; - packet->heading = heading; - packet->throttle = throttle; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE VFR_HUD UNPACKING - - -/** - * @brief Get field airspeed from vfr_hud message - * - * @return Current airspeed in m/s - */ -static inline float mavlink_msg_vfr_hud_get_airspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field groundspeed from vfr_hud message - * - * @return Current ground speed in m/s - */ -static inline float mavlink_msg_vfr_hud_get_groundspeed(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field heading from vfr_hud message - * - * @return Current heading in degrees, in compass units (0..360, 0=north) - */ -static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 16); -} - -/** - * @brief Get field throttle from vfr_hud message - * - * @return Current throttle setting in integer percent, 0 to 100 - */ -static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 18); -} - -/** - * @brief Get field alt from vfr_hud message - * - * @return Current altitude (MSL), in meters - */ -static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field climb from vfr_hud message - * - * @return Current climb rate in meters/second - */ -static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a vfr_hud message into a struct - * - * @param msg The message to decode - * @param vfr_hud C-struct to decode the message contents into - */ -static inline void mavlink_msg_vfr_hud_decode(const mavlink_message_t* msg, mavlink_vfr_hud_t* vfr_hud) -{ -#if MAVLINK_NEED_BYTE_SWAP - vfr_hud->airspeed = mavlink_msg_vfr_hud_get_airspeed(msg); - vfr_hud->groundspeed = mavlink_msg_vfr_hud_get_groundspeed(msg); - vfr_hud->alt = mavlink_msg_vfr_hud_get_alt(msg); - vfr_hud->climb = mavlink_msg_vfr_hud_get_climb(msg); - vfr_hud->heading = mavlink_msg_vfr_hud_get_heading(msg); - vfr_hud->throttle = mavlink_msg_vfr_hud_get_throttle(msg); -#else - memcpy(vfr_hud, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_VFR_HUD_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h deleted file mode 100644 index b3fa7bc1c..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE VICON_POSITION_ESTIMATE PACKING - -#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE 104 - -typedef struct __mavlink_vicon_position_estimate_t -{ - uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) - float x; ///< Global X position - float y; ///< Global Y position - float z; ///< Global Z position - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad -} mavlink_vicon_position_estimate_t; - -#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 32 -#define MAVLINK_MSG_ID_104_LEN 32 - -#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC 56 -#define MAVLINK_MSG_ID_104_CRC 56 - - - -#define MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE { \ - "VICON_POSITION_ESTIMATE", \ - 7, \ - { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vicon_position_estimate_t, usec) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vicon_position_estimate_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vicon_position_estimate_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vicon_position_estimate_t, z) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vicon_position_estimate_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vicon_position_estimate_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vicon_position_estimate_t, yaw) }, \ - } \ -} - - -/** - * @brief Pack a vicon_position_estimate message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, roll); - _mav_put_float(buf, 24, pitch); - _mav_put_float(buf, 28, yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); -#else - mavlink_vicon_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); -#endif -} - -/** - * @brief Pack a vicon_position_estimate message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, roll); - _mav_put_float(buf, 24, pitch); - _mav_put_float(buf, 28, yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); -#else - mavlink_vicon_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); -#endif -} - -/** - * @brief Encode a vicon_position_estimate struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param vicon_position_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) -{ - return mavlink_msg_vicon_position_estimate_pack(system_id, component_id, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); -} - -/** - * @brief Encode a vicon_position_estimate struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param vicon_position_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vicon_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) -{ - return mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, chan, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); -} - -/** - * @brief Send a vicon_position_estimate message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, roll); - _mav_put_float(buf, 24, pitch); - _mav_put_float(buf, 28, yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); -#endif -#else - mavlink_vicon_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, roll); - _mav_put_float(buf, 24, pitch); - _mav_put_float(buf, 28, yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); -#endif -#else - mavlink_vicon_position_estimate_t *packet = (mavlink_vicon_position_estimate_t *)msgbuf; - packet->usec = usec; - packet->x = x; - packet->y = y; - packet->z = z; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE VICON_POSITION_ESTIMATE UNPACKING - - -/** - * @brief Get field usec from vicon_position_estimate message - * - * @return Timestamp (microseconds, synced to UNIX time or since system boot) - */ -static inline uint64_t mavlink_msg_vicon_position_estimate_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field x from vicon_position_estimate message - * - * @return Global X position - */ -static inline float mavlink_msg_vicon_position_estimate_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field y from vicon_position_estimate message - * - * @return Global Y position - */ -static inline float mavlink_msg_vicon_position_estimate_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field z from vicon_position_estimate message - * - * @return Global Z position - */ -static inline float mavlink_msg_vicon_position_estimate_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field roll from vicon_position_estimate message - * - * @return Roll angle in rad - */ -static inline float mavlink_msg_vicon_position_estimate_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field pitch from vicon_position_estimate message - * - * @return Pitch angle in rad - */ -static inline float mavlink_msg_vicon_position_estimate_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field yaw from vicon_position_estimate message - * - * @return Yaw angle in rad - */ -static inline float mavlink_msg_vicon_position_estimate_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a vicon_position_estimate message into a struct - * - * @param msg The message to decode - * @param vicon_position_estimate C-struct to decode the message contents into - */ -static inline void mavlink_msg_vicon_position_estimate_decode(const mavlink_message_t* msg, mavlink_vicon_position_estimate_t* vicon_position_estimate) -{ -#if MAVLINK_NEED_BYTE_SWAP - vicon_position_estimate->usec = mavlink_msg_vicon_position_estimate_get_usec(msg); - vicon_position_estimate->x = mavlink_msg_vicon_position_estimate_get_x(msg); - vicon_position_estimate->y = mavlink_msg_vicon_position_estimate_get_y(msg); - vicon_position_estimate->z = mavlink_msg_vicon_position_estimate_get_z(msg); - vicon_position_estimate->roll = mavlink_msg_vicon_position_estimate_get_roll(msg); - vicon_position_estimate->pitch = mavlink_msg_vicon_position_estimate_get_pitch(msg); - vicon_position_estimate->yaw = mavlink_msg_vicon_position_estimate_get_yaw(msg); -#else - memcpy(vicon_position_estimate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h deleted file mode 100644 index 8f82fb682..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE VISION_POSITION_ESTIMATE PACKING - -#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE 102 - -typedef struct __mavlink_vision_position_estimate_t -{ - uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) - float x; ///< Global X position - float y; ///< Global Y position - float z; ///< Global Z position - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad -} mavlink_vision_position_estimate_t; - -#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 32 -#define MAVLINK_MSG_ID_102_LEN 32 - -#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC 158 -#define MAVLINK_MSG_ID_102_CRC 158 - - - -#define MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE { \ - "VISION_POSITION_ESTIMATE", \ - 7, \ - { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_position_estimate_t, usec) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_position_estimate_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_position_estimate_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_position_estimate_t, z) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vision_position_estimate_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vision_position_estimate_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vision_position_estimate_t, yaw) }, \ - } \ -} - - -/** - * @brief Pack a vision_position_estimate message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, roll); - _mav_put_float(buf, 24, pitch); - _mav_put_float(buf, 28, yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); -#else - mavlink_vision_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); -#endif -} - -/** - * @brief Pack a vision_position_estimate message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, roll); - _mav_put_float(buf, 24, pitch); - _mav_put_float(buf, 28, yaw); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); -#else - mavlink_vision_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); -#endif -} - -/** - * @brief Encode a vision_position_estimate struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param vision_position_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) -{ - return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); -} - -/** - * @brief Encode a vision_position_estimate struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param vision_position_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) -{ - return mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); -} - -/** - * @brief Send a vision_position_estimate message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, roll); - _mav_put_float(buf, 24, pitch); - _mav_put_float(buf, 28, yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); -#endif -#else - mavlink_vision_position_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, roll); - _mav_put_float(buf, 24, pitch); - _mav_put_float(buf, 28, yaw); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); -#endif -#else - mavlink_vision_position_estimate_t *packet = (mavlink_vision_position_estimate_t *)msgbuf; - packet->usec = usec; - packet->x = x; - packet->y = y; - packet->z = z; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE VISION_POSITION_ESTIMATE UNPACKING - - -/** - * @brief Get field usec from vision_position_estimate message - * - * @return Timestamp (microseconds, synced to UNIX time or since system boot) - */ -static inline uint64_t mavlink_msg_vision_position_estimate_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field x from vision_position_estimate message - * - * @return Global X position - */ -static inline float mavlink_msg_vision_position_estimate_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field y from vision_position_estimate message - * - * @return Global Y position - */ -static inline float mavlink_msg_vision_position_estimate_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field z from vision_position_estimate message - * - * @return Global Z position - */ -static inline float mavlink_msg_vision_position_estimate_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field roll from vision_position_estimate message - * - * @return Roll angle in rad - */ -static inline float mavlink_msg_vision_position_estimate_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field pitch from vision_position_estimate message - * - * @return Pitch angle in rad - */ -static inline float mavlink_msg_vision_position_estimate_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field yaw from vision_position_estimate message - * - * @return Yaw angle in rad - */ -static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a vision_position_estimate message into a struct - * - * @param msg The message to decode - * @param vision_position_estimate C-struct to decode the message contents into - */ -static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_vision_position_estimate_t* vision_position_estimate) -{ -#if MAVLINK_NEED_BYTE_SWAP - vision_position_estimate->usec = mavlink_msg_vision_position_estimate_get_usec(msg); - vision_position_estimate->x = mavlink_msg_vision_position_estimate_get_x(msg); - vision_position_estimate->y = mavlink_msg_vision_position_estimate_get_y(msg); - vision_position_estimate->z = mavlink_msg_vision_position_estimate_get_z(msg); - vision_position_estimate->roll = mavlink_msg_vision_position_estimate_get_roll(msg); - vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg); - vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg); -#else - memcpy(vision_position_estimate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h deleted file mode 100644 index 7528014c7..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE VISION_SPEED_ESTIMATE PACKING - -#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 103 - -typedef struct __mavlink_vision_speed_estimate_t -{ - uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) - float x; ///< Global X speed - float y; ///< Global Y speed - float z; ///< Global Z speed -} mavlink_vision_speed_estimate_t; - -#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 20 -#define MAVLINK_MSG_ID_103_LEN 20 - -#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC 208 -#define MAVLINK_MSG_ID_103_CRC 208 - - - -#define MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE { \ - "VISION_SPEED_ESTIMATE", \ - 4, \ - { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_speed_estimate_t, usec) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_speed_estimate_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_speed_estimate_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_speed_estimate_t, z) }, \ - } \ -} - - -/** - * @brief Pack a vision_speed_estimate message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param x Global X speed - * @param y Global Y speed - * @param z Global Z speed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); -#else - mavlink_vision_speed_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); -#endif -} - -/** - * @brief Pack a vision_speed_estimate message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param x Global X speed - * @param y Global Y speed - * @param z Global Z speed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t usec,float x,float y,float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); -#else - mavlink_vision_speed_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); -#endif -} - -/** - * @brief Encode a vision_speed_estimate struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param vision_speed_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) -{ - return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); -} - -/** - * @brief Encode a vision_speed_estimate struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param vision_speed_estimate C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_vision_speed_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) -{ - return mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, chan, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); -} - -/** - * @brief Send a vision_speed_estimate message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) - * @param x Global X speed - * @param y Global Y speed - * @param z Global Z speed - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); -#endif -#else - mavlink_vision_speed_estimate_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); -#endif -#else - mavlink_vision_speed_estimate_t *packet = (mavlink_vision_speed_estimate_t *)msgbuf; - packet->usec = usec; - packet->x = x; - packet->y = y; - packet->z = z; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE VISION_SPEED_ESTIMATE UNPACKING - - -/** - * @brief Get field usec from vision_speed_estimate message - * - * @return Timestamp (microseconds, synced to UNIX time or since system boot) - */ -static inline uint64_t mavlink_msg_vision_speed_estimate_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field x from vision_speed_estimate message - * - * @return Global X speed - */ -static inline float mavlink_msg_vision_speed_estimate_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field y from vision_speed_estimate message - * - * @return Global Y speed - */ -static inline float mavlink_msg_vision_speed_estimate_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field z from vision_speed_estimate message - * - * @return Global Z speed - */ -static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a vision_speed_estimate message into a struct - * - * @param msg The message to decode - * @param vision_speed_estimate C-struct to decode the message contents into - */ -static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_message_t* msg, mavlink_vision_speed_estimate_t* vision_speed_estimate) -{ -#if MAVLINK_NEED_BYTE_SWAP - vision_speed_estimate->usec = mavlink_msg_vision_speed_estimate_get_usec(msg); - vision_speed_estimate->x = mavlink_msg_vision_speed_estimate_get_x(msg); - vision_speed_estimate->y = mavlink_msg_vision_speed_estimate_get_y(msg); - vision_speed_estimate->z = mavlink_msg_vision_speed_estimate_get_z(msg); -#else - memcpy(vision_speed_estimate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h deleted file mode 100644 index de23fcb2a..000000000 --- a/mavlink/include/mavlink/v1.0/common/testsuite.h +++ /dev/null @@ -1,5872 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from common.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef COMMON_TESTSUITE_H -#define COMMON_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL - -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); - -static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - - mavlink_test_common(system_id, component_id, last_msg); -} -#endif - - - - -static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_heartbeat_t packet_in = { - 963497464, - }17, - }84, - }151, - }218, - }3, - }; - mavlink_heartbeat_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.custom_mode = packet_in.custom_mode; - packet1.type = packet_in.type; - packet1.autopilot = packet_in.autopilot; - packet1.base_mode = packet_in.base_mode; - packet1.system_status = packet_in.system_status; - packet1.mavlink_version = packet_in.mavlink_version; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_heartbeat_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); - mavlink_msg_heartbeat_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); - mavlink_msg_heartbeat_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; i 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ - MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ - MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ - MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_ENUM_END=501, /* | */ -}; -#endif - -#include "../common/common.h" - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_flexifunction_set.h" -#include "./mavlink_msg_flexifunction_read_req.h" -#include "./mavlink_msg_flexifunction_buffer_function.h" -#include "./mavlink_msg_flexifunction_buffer_function_ack.h" -#include "./mavlink_msg_flexifunction_directory.h" -#include "./mavlink_msg_flexifunction_directory_ack.h" -#include "./mavlink_msg_flexifunction_command.h" -#include "./mavlink_msg_flexifunction_command_ack.h" -#include "./mavlink_msg_serial_udb_extra_f2_a.h" -#include "./mavlink_msg_serial_udb_extra_f2_b.h" -#include "./mavlink_msg_serial_udb_extra_f4.h" -#include "./mavlink_msg_serial_udb_extra_f5.h" -#include "./mavlink_msg_serial_udb_extra_f6.h" -#include "./mavlink_msg_serial_udb_extra_f7.h" -#include "./mavlink_msg_serial_udb_extra_f8.h" -#include "./mavlink_msg_serial_udb_extra_f13.h" -#include "./mavlink_msg_serial_udb_extra_f14.h" -#include "./mavlink_msg_serial_udb_extra_f15.h" -#include "./mavlink_msg_serial_udb_extra_f16.h" -#include "./mavlink_msg_altitudes.h" -#include "./mavlink_msg_airspeeds.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // MATRIXPILOT_H diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h deleted file mode 100644 index 7e62a9719..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h +++ /dev/null @@ -1,27 +0,0 @@ -/** @file - * @brief MAVLink comm protocol built from matrixpilot.xml - * @see http://pixhawk.ethz.ch/software/mavlink - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#ifndef MAVLINK_STX -#define MAVLINK_STX 254 -#endif - -#ifndef MAVLINK_ENDIAN -#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN -#endif - -#ifndef MAVLINK_ALIGNED_FIELDS -#define MAVLINK_ALIGNED_FIELDS 1 -#endif - -#ifndef MAVLINK_CRC_EXTRA -#define MAVLINK_CRC_EXTRA 1 -#endif - -#include "version.h" -#include "matrixpilot.h" - -#endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h deleted file mode 100644 index c834a3df0..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE AIRSPEEDS PACKING - -#define MAVLINK_MSG_ID_AIRSPEEDS 182 - -typedef struct __mavlink_airspeeds_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - int16_t airspeed_imu; ///< Airspeed estimate from IMU, cm/s - int16_t airspeed_pitot; ///< Pitot measured forward airpseed, cm/s - int16_t airspeed_hot_wire; ///< Hot wire anenometer measured airspeed, cm/s - int16_t airspeed_ultrasonic; ///< Ultrasonic measured airspeed, cm/s - int16_t aoa; ///< Angle of attack sensor, degrees * 10 - int16_t aoy; ///< Yaw angle sensor, degrees * 10 -} mavlink_airspeeds_t; - -#define MAVLINK_MSG_ID_AIRSPEEDS_LEN 16 -#define MAVLINK_MSG_ID_182_LEN 16 - -#define MAVLINK_MSG_ID_AIRSPEEDS_CRC 154 -#define MAVLINK_MSG_ID_182_CRC 154 - - - -#define MAVLINK_MESSAGE_INFO_AIRSPEEDS { \ - "AIRSPEEDS", \ - 7, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_airspeeds_t, time_boot_ms) }, \ - { "airspeed_imu", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_airspeeds_t, airspeed_imu) }, \ - { "airspeed_pitot", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_airspeeds_t, airspeed_pitot) }, \ - { "airspeed_hot_wire", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_airspeeds_t, airspeed_hot_wire) }, \ - { "airspeed_ultrasonic", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_airspeeds_t, airspeed_ultrasonic) }, \ - { "aoa", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_airspeeds_t, aoa) }, \ - { "aoy", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_airspeeds_t, aoy) }, \ - } \ -} - - -/** - * @brief Pack a airspeeds message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param airspeed_imu Airspeed estimate from IMU, cm/s - * @param airspeed_pitot Pitot measured forward airpseed, cm/s - * @param airspeed_hot_wire Hot wire anenometer measured airspeed, cm/s - * @param airspeed_ultrasonic Ultrasonic measured airspeed, cm/s - * @param aoa Angle of attack sensor, degrees * 10 - * @param aoy Yaw angle sensor, degrees * 10 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_airspeeds_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int16_t airspeed_imu, int16_t airspeed_pitot, int16_t airspeed_hot_wire, int16_t airspeed_ultrasonic, int16_t aoa, int16_t aoy) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AIRSPEEDS_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, airspeed_imu); - _mav_put_int16_t(buf, 6, airspeed_pitot); - _mav_put_int16_t(buf, 8, airspeed_hot_wire); - _mav_put_int16_t(buf, 10, airspeed_ultrasonic); - _mav_put_int16_t(buf, 12, aoa); - _mav_put_int16_t(buf, 14, aoy); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#else - mavlink_airspeeds_t packet; - packet.time_boot_ms = time_boot_ms; - packet.airspeed_imu = airspeed_imu; - packet.airspeed_pitot = airspeed_pitot; - packet.airspeed_hot_wire = airspeed_hot_wire; - packet.airspeed_ultrasonic = airspeed_ultrasonic; - packet.aoa = aoa; - packet.aoy = aoy; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AIRSPEEDS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif -} - -/** - * @brief Pack a airspeeds message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param airspeed_imu Airspeed estimate from IMU, cm/s - * @param airspeed_pitot Pitot measured forward airpseed, cm/s - * @param airspeed_hot_wire Hot wire anenometer measured airspeed, cm/s - * @param airspeed_ultrasonic Ultrasonic measured airspeed, cm/s - * @param aoa Angle of attack sensor, degrees * 10 - * @param aoy Yaw angle sensor, degrees * 10 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_airspeeds_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,int16_t airspeed_imu,int16_t airspeed_pitot,int16_t airspeed_hot_wire,int16_t airspeed_ultrasonic,int16_t aoa,int16_t aoy) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AIRSPEEDS_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, airspeed_imu); - _mav_put_int16_t(buf, 6, airspeed_pitot); - _mav_put_int16_t(buf, 8, airspeed_hot_wire); - _mav_put_int16_t(buf, 10, airspeed_ultrasonic); - _mav_put_int16_t(buf, 12, aoa); - _mav_put_int16_t(buf, 14, aoy); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#else - mavlink_airspeeds_t packet; - packet.time_boot_ms = time_boot_ms; - packet.airspeed_imu = airspeed_imu; - packet.airspeed_pitot = airspeed_pitot; - packet.airspeed_hot_wire = airspeed_hot_wire; - packet.airspeed_ultrasonic = airspeed_ultrasonic; - packet.aoa = aoa; - packet.aoy = aoy; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_AIRSPEEDS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif -} - -/** - * @brief Encode a airspeeds struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param airspeeds C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_airspeeds_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_airspeeds_t* airspeeds) -{ - return mavlink_msg_airspeeds_pack(system_id, component_id, msg, airspeeds->time_boot_ms, airspeeds->airspeed_imu, airspeeds->airspeed_pitot, airspeeds->airspeed_hot_wire, airspeeds->airspeed_ultrasonic, airspeeds->aoa, airspeeds->aoy); -} - -/** - * @brief Encode a airspeeds struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param airspeeds C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_airspeeds_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeeds_t* airspeeds) -{ - return mavlink_msg_airspeeds_pack_chan(system_id, component_id, chan, msg, airspeeds->time_boot_ms, airspeeds->airspeed_imu, airspeeds->airspeed_pitot, airspeeds->airspeed_hot_wire, airspeeds->airspeed_ultrasonic, airspeeds->aoa, airspeeds->aoy); -} - -/** - * @brief Send a airspeeds message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param airspeed_imu Airspeed estimate from IMU, cm/s - * @param airspeed_pitot Pitot measured forward airpseed, cm/s - * @param airspeed_hot_wire Hot wire anenometer measured airspeed, cm/s - * @param airspeed_ultrasonic Ultrasonic measured airspeed, cm/s - * @param aoa Angle of attack sensor, degrees * 10 - * @param aoy Yaw angle sensor, degrees * 10 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_airspeeds_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t airspeed_imu, int16_t airspeed_pitot, int16_t airspeed_hot_wire, int16_t airspeed_ultrasonic, int16_t aoa, int16_t aoy) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_AIRSPEEDS_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, airspeed_imu); - _mav_put_int16_t(buf, 6, airspeed_pitot); - _mav_put_int16_t(buf, 8, airspeed_hot_wire); - _mav_put_int16_t(buf, 10, airspeed_ultrasonic); - _mav_put_int16_t(buf, 12, aoa); - _mav_put_int16_t(buf, 14, aoy); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif -#else - mavlink_airspeeds_t packet; - packet.time_boot_ms = time_boot_ms; - packet.airspeed_imu = airspeed_imu; - packet.airspeed_pitot = airspeed_pitot; - packet.airspeed_hot_wire = airspeed_hot_wire; - packet.airspeed_ultrasonic = airspeed_ultrasonic; - packet.aoa = aoa; - packet.aoy = aoy; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_AIRSPEEDS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_airspeeds_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t airspeed_imu, int16_t airspeed_pitot, int16_t airspeed_hot_wire, int16_t airspeed_ultrasonic, int16_t aoa, int16_t aoy) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int16_t(buf, 4, airspeed_imu); - _mav_put_int16_t(buf, 6, airspeed_pitot); - _mav_put_int16_t(buf, 8, airspeed_hot_wire); - _mav_put_int16_t(buf, 10, airspeed_ultrasonic); - _mav_put_int16_t(buf, 12, aoa); - _mav_put_int16_t(buf, 14, aoy); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif -#else - mavlink_airspeeds_t *packet = (mavlink_airspeeds_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->airspeed_imu = airspeed_imu; - packet->airspeed_pitot = airspeed_pitot; - packet->airspeed_hot_wire = airspeed_hot_wire; - packet->airspeed_ultrasonic = airspeed_ultrasonic; - packet->aoa = aoa; - packet->aoy = aoy; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE AIRSPEEDS UNPACKING - - -/** - * @brief Get field time_boot_ms from airspeeds message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_airspeeds_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field airspeed_imu from airspeeds message - * - * @return Airspeed estimate from IMU, cm/s - */ -static inline int16_t mavlink_msg_airspeeds_get_airspeed_imu(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 4); -} - -/** - * @brief Get field airspeed_pitot from airspeeds message - * - * @return Pitot measured forward airpseed, cm/s - */ -static inline int16_t mavlink_msg_airspeeds_get_airspeed_pitot(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 6); -} - -/** - * @brief Get field airspeed_hot_wire from airspeeds message - * - * @return Hot wire anenometer measured airspeed, cm/s - */ -static inline int16_t mavlink_msg_airspeeds_get_airspeed_hot_wire(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field airspeed_ultrasonic from airspeeds message - * - * @return Ultrasonic measured airspeed, cm/s - */ -static inline int16_t mavlink_msg_airspeeds_get_airspeed_ultrasonic(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 10); -} - -/** - * @brief Get field aoa from airspeeds message - * - * @return Angle of attack sensor, degrees * 10 - */ -static inline int16_t mavlink_msg_airspeeds_get_aoa(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Get field aoy from airspeeds message - * - * @return Yaw angle sensor, degrees * 10 - */ -static inline int16_t mavlink_msg_airspeeds_get_aoy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 14); -} - -/** - * @brief Decode a airspeeds message into a struct - * - * @param msg The message to decode - * @param airspeeds C-struct to decode the message contents into - */ -static inline void mavlink_msg_airspeeds_decode(const mavlink_message_t* msg, mavlink_airspeeds_t* airspeeds) -{ -#if MAVLINK_NEED_BYTE_SWAP - airspeeds->time_boot_ms = mavlink_msg_airspeeds_get_time_boot_ms(msg); - airspeeds->airspeed_imu = mavlink_msg_airspeeds_get_airspeed_imu(msg); - airspeeds->airspeed_pitot = mavlink_msg_airspeeds_get_airspeed_pitot(msg); - airspeeds->airspeed_hot_wire = mavlink_msg_airspeeds_get_airspeed_hot_wire(msg); - airspeeds->airspeed_ultrasonic = mavlink_msg_airspeeds_get_airspeed_ultrasonic(msg); - airspeeds->aoa = mavlink_msg_airspeeds_get_aoa(msg); - airspeeds->aoy = mavlink_msg_airspeeds_get_aoy(msg); -#else - memcpy(airspeeds, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AIRSPEEDS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h deleted file mode 100644 index b009068ff..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE ALTITUDES PACKING - -#define MAVLINK_MSG_ID_ALTITUDES 181 - -typedef struct __mavlink_altitudes_t -{ - uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - int32_t alt_gps; ///< GPS altitude in meters, expressed as * 1000 (millimeters), above MSL - int32_t alt_imu; ///< IMU altitude above ground in meters, expressed as * 1000 (millimeters) - int32_t alt_barometric; ///< barometeric altitude above ground in meters, expressed as * 1000 (millimeters) - int32_t alt_optical_flow; ///< Optical flow altitude above ground in meters, expressed as * 1000 (millimeters) - int32_t alt_range_finder; ///< Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters) - int32_t alt_extra; ///< Extra altitude above ground in meters, expressed as * 1000 (millimeters) -} mavlink_altitudes_t; - -#define MAVLINK_MSG_ID_ALTITUDES_LEN 28 -#define MAVLINK_MSG_ID_181_LEN 28 - -#define MAVLINK_MSG_ID_ALTITUDES_CRC 55 -#define MAVLINK_MSG_ID_181_CRC 55 - - - -#define MAVLINK_MESSAGE_INFO_ALTITUDES { \ - "ALTITUDES", \ - 7, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_altitudes_t, time_boot_ms) }, \ - { "alt_gps", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_altitudes_t, alt_gps) }, \ - { "alt_imu", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_altitudes_t, alt_imu) }, \ - { "alt_barometric", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_altitudes_t, alt_barometric) }, \ - { "alt_optical_flow", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_altitudes_t, alt_optical_flow) }, \ - { "alt_range_finder", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_altitudes_t, alt_range_finder) }, \ - { "alt_extra", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_altitudes_t, alt_extra) }, \ - } \ -} - - -/** - * @brief Pack a altitudes message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param alt_gps GPS altitude in meters, expressed as * 1000 (millimeters), above MSL - * @param alt_imu IMU altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_barometric barometeric altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_optical_flow Optical flow altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_range_finder Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_extra Extra altitude above ground in meters, expressed as * 1000 (millimeters) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_altitudes_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int32_t alt_gps, int32_t alt_imu, int32_t alt_barometric, int32_t alt_optical_flow, int32_t alt_range_finder, int32_t alt_extra) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ALTITUDES_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, alt_gps); - _mav_put_int32_t(buf, 8, alt_imu); - _mav_put_int32_t(buf, 12, alt_barometric); - _mav_put_int32_t(buf, 16, alt_optical_flow); - _mav_put_int32_t(buf, 20, alt_range_finder); - _mav_put_int32_t(buf, 24, alt_extra); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ALTITUDES_LEN); -#else - mavlink_altitudes_t packet; - packet.time_boot_ms = time_boot_ms; - packet.alt_gps = alt_gps; - packet.alt_imu = alt_imu; - packet.alt_barometric = alt_barometric; - packet.alt_optical_flow = alt_optical_flow; - packet.alt_range_finder = alt_range_finder; - packet.alt_extra = alt_extra; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ALTITUDES; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif -} - -/** - * @brief Pack a altitudes message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param alt_gps GPS altitude in meters, expressed as * 1000 (millimeters), above MSL - * @param alt_imu IMU altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_barometric barometeric altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_optical_flow Optical flow altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_range_finder Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_extra Extra altitude above ground in meters, expressed as * 1000 (millimeters) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_altitudes_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,int32_t alt_gps,int32_t alt_imu,int32_t alt_barometric,int32_t alt_optical_flow,int32_t alt_range_finder,int32_t alt_extra) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ALTITUDES_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, alt_gps); - _mav_put_int32_t(buf, 8, alt_imu); - _mav_put_int32_t(buf, 12, alt_barometric); - _mav_put_int32_t(buf, 16, alt_optical_flow); - _mav_put_int32_t(buf, 20, alt_range_finder); - _mav_put_int32_t(buf, 24, alt_extra); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ALTITUDES_LEN); -#else - mavlink_altitudes_t packet; - packet.time_boot_ms = time_boot_ms; - packet.alt_gps = alt_gps; - packet.alt_imu = alt_imu; - packet.alt_barometric = alt_barometric; - packet.alt_optical_flow = alt_optical_flow; - packet.alt_range_finder = alt_range_finder; - packet.alt_extra = alt_extra; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ALTITUDES; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif -} - -/** - * @brief Encode a altitudes struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param altitudes C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_altitudes_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_altitudes_t* altitudes) -{ - return mavlink_msg_altitudes_pack(system_id, component_id, msg, altitudes->time_boot_ms, altitudes->alt_gps, altitudes->alt_imu, altitudes->alt_barometric, altitudes->alt_optical_flow, altitudes->alt_range_finder, altitudes->alt_extra); -} - -/** - * @brief Encode a altitudes struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param altitudes C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_altitudes_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_altitudes_t* altitudes) -{ - return mavlink_msg_altitudes_pack_chan(system_id, component_id, chan, msg, altitudes->time_boot_ms, altitudes->alt_gps, altitudes->alt_imu, altitudes->alt_barometric, altitudes->alt_optical_flow, altitudes->alt_range_finder, altitudes->alt_extra); -} - -/** - * @brief Send a altitudes message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param alt_gps GPS altitude in meters, expressed as * 1000 (millimeters), above MSL - * @param alt_imu IMU altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_barometric barometeric altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_optical_flow Optical flow altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_range_finder Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters) - * @param alt_extra Extra altitude above ground in meters, expressed as * 1000 (millimeters) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_altitudes_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t alt_gps, int32_t alt_imu, int32_t alt_barometric, int32_t alt_optical_flow, int32_t alt_range_finder, int32_t alt_extra) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ALTITUDES_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, alt_gps); - _mav_put_int32_t(buf, 8, alt_imu); - _mav_put_int32_t(buf, 12, alt_barometric); - _mav_put_int32_t(buf, 16, alt_optical_flow); - _mav_put_int32_t(buf, 20, alt_range_finder); - _mav_put_int32_t(buf, 24, alt_extra); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif -#else - mavlink_altitudes_t packet; - packet.time_boot_ms = time_boot_ms; - packet.alt_gps = alt_gps; - packet.alt_imu = alt_imu; - packet.alt_barometric = alt_barometric; - packet.alt_optical_flow = alt_optical_flow; - packet.alt_range_finder = alt_range_finder; - packet.alt_extra = alt_extra; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)&packet, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)&packet, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_ALTITUDES_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_altitudes_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t alt_gps, int32_t alt_imu, int32_t alt_barometric, int32_t alt_optical_flow, int32_t alt_range_finder, int32_t alt_extra) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, alt_gps); - _mav_put_int32_t(buf, 8, alt_imu); - _mav_put_int32_t(buf, 12, alt_barometric); - _mav_put_int32_t(buf, 16, alt_optical_flow); - _mav_put_int32_t(buf, 20, alt_range_finder); - _mav_put_int32_t(buf, 24, alt_extra); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif -#else - mavlink_altitudes_t *packet = (mavlink_altitudes_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->alt_gps = alt_gps; - packet->alt_imu = alt_imu; - packet->alt_barometric = alt_barometric; - packet->alt_optical_flow = alt_optical_flow; - packet->alt_range_finder = alt_range_finder; - packet->alt_extra = alt_extra; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)packet, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)packet, MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE ALTITUDES UNPACKING - - -/** - * @brief Get field time_boot_ms from altitudes message - * - * @return Timestamp (milliseconds since system boot) - */ -static inline uint32_t mavlink_msg_altitudes_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field alt_gps from altitudes message - * - * @return GPS altitude in meters, expressed as * 1000 (millimeters), above MSL - */ -static inline int32_t mavlink_msg_altitudes_get_alt_gps(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field alt_imu from altitudes message - * - * @return IMU altitude above ground in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_altitudes_get_alt_imu(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field alt_barometric from altitudes message - * - * @return barometeric altitude above ground in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_altitudes_get_alt_barometric(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 12); -} - -/** - * @brief Get field alt_optical_flow from altitudes message - * - * @return Optical flow altitude above ground in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_altitudes_get_alt_optical_flow(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 16); -} - -/** - * @brief Get field alt_range_finder from altitudes message - * - * @return Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_altitudes_get_alt_range_finder(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 20); -} - -/** - * @brief Get field alt_extra from altitudes message - * - * @return Extra altitude above ground in meters, expressed as * 1000 (millimeters) - */ -static inline int32_t mavlink_msg_altitudes_get_alt_extra(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 24); -} - -/** - * @brief Decode a altitudes message into a struct - * - * @param msg The message to decode - * @param altitudes C-struct to decode the message contents into - */ -static inline void mavlink_msg_altitudes_decode(const mavlink_message_t* msg, mavlink_altitudes_t* altitudes) -{ -#if MAVLINK_NEED_BYTE_SWAP - altitudes->time_boot_ms = mavlink_msg_altitudes_get_time_boot_ms(msg); - altitudes->alt_gps = mavlink_msg_altitudes_get_alt_gps(msg); - altitudes->alt_imu = mavlink_msg_altitudes_get_alt_imu(msg); - altitudes->alt_barometric = mavlink_msg_altitudes_get_alt_barometric(msg); - altitudes->alt_optical_flow = mavlink_msg_altitudes_get_alt_optical_flow(msg); - altitudes->alt_range_finder = mavlink_msg_altitudes_get_alt_range_finder(msg); - altitudes->alt_extra = mavlink_msg_altitudes_get_alt_extra(msg); -#else - memcpy(altitudes, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ALTITUDES_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h deleted file mode 100644 index e7b803a14..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h +++ /dev/null @@ -1,345 +0,0 @@ -// MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION 152 - -typedef struct __mavlink_flexifunction_buffer_function_t -{ - uint16_t func_index; ///< Function index - uint16_t func_count; ///< Total count of functions - uint16_t data_address; ///< Address in the flexifunction data, Set to 0xFFFF to use address in target memory - uint16_t data_size; ///< Size of the - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - int8_t data[48]; ///< Settings data -} mavlink_flexifunction_buffer_function_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN 58 -#define MAVLINK_MSG_ID_152_LEN 58 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC 101 -#define MAVLINK_MSG_ID_152_CRC 101 - -#define MAVLINK_MSG_FLEXIFUNCTION_BUFFER_FUNCTION_FIELD_DATA_LEN 48 - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_BUFFER_FUNCTION { \ - "FLEXIFUNCTION_BUFFER_FUNCTION", \ - 7, \ - { { "func_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_flexifunction_buffer_function_t, func_index) }, \ - { "func_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_flexifunction_buffer_function_t, func_count) }, \ - { "data_address", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_flexifunction_buffer_function_t, data_address) }, \ - { "data_size", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_flexifunction_buffer_function_t, data_size) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_flexifunction_buffer_function_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_flexifunction_buffer_function_t, target_component) }, \ - { "data", NULL, MAVLINK_TYPE_INT8_T, 48, 10, offsetof(mavlink_flexifunction_buffer_function_t, data) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_buffer_function message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param func_index Function index - * @param func_count Total count of functions - * @param data_address Address in the flexifunction data, Set to 0xFFFF to use address in target memory - * @param data_size Size of the - * @param data Settings data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t func_count, uint16_t data_address, uint16_t data_size, const int8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN]; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, func_count); - _mav_put_uint16_t(buf, 4, data_address); - _mav_put_uint16_t(buf, 6, data_size); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_int8_t_array(buf, 10, data, 48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#else - mavlink_flexifunction_buffer_function_t packet; - packet.func_index = func_index; - packet.func_count = func_count; - packet.data_address = data_address; - packet.data_size = data_size; - packet.target_system = target_system; - packet.target_component = target_component; - mav_array_memcpy(packet.data, data, sizeof(int8_t)*48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_buffer_function message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param func_index Function index - * @param func_count Total count of functions - * @param data_address Address in the flexifunction data, Set to 0xFFFF to use address in target memory - * @param data_size Size of the - * @param data Settings data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t func_index,uint16_t func_count,uint16_t data_address,uint16_t data_size,const int8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN]; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, func_count); - _mav_put_uint16_t(buf, 4, data_address); - _mav_put_uint16_t(buf, 6, data_size); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_int8_t_array(buf, 10, data, 48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#else - mavlink_flexifunction_buffer_function_t packet; - packet.func_index = func_index; - packet.func_count = func_count; - packet.data_address = data_address; - packet.data_size = data_size; - packet.target_system = target_system; - packet.target_component = target_component; - mav_array_memcpy(packet.data, data, sizeof(int8_t)*48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_buffer_function struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_buffer_function C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_t* flexifunction_buffer_function) -{ - return mavlink_msg_flexifunction_buffer_function_pack(system_id, component_id, msg, flexifunction_buffer_function->target_system, flexifunction_buffer_function->target_component, flexifunction_buffer_function->func_index, flexifunction_buffer_function->func_count, flexifunction_buffer_function->data_address, flexifunction_buffer_function->data_size, flexifunction_buffer_function->data); -} - -/** - * @brief Encode a flexifunction_buffer_function struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_buffer_function C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_t* flexifunction_buffer_function) -{ - return mavlink_msg_flexifunction_buffer_function_pack_chan(system_id, component_id, chan, msg, flexifunction_buffer_function->target_system, flexifunction_buffer_function->target_component, flexifunction_buffer_function->func_index, flexifunction_buffer_function->func_count, flexifunction_buffer_function->data_address, flexifunction_buffer_function->data_size, flexifunction_buffer_function->data); -} - -/** - * @brief Send a flexifunction_buffer_function message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param func_index Function index - * @param func_count Total count of functions - * @param data_address Address in the flexifunction data, Set to 0xFFFF to use address in target memory - * @param data_size Size of the - * @param data Settings data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_buffer_function_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t func_count, uint16_t data_address, uint16_t data_size, const int8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN]; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, func_count); - _mav_put_uint16_t(buf, 4, data_address); - _mav_put_uint16_t(buf, 6, data_size); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_int8_t_array(buf, 10, data, 48); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif -#else - mavlink_flexifunction_buffer_function_t packet; - packet.func_index = func_index; - packet.func_count = func_count; - packet.data_address = data_address; - packet.data_size = data_size; - packet.target_system = target_system; - packet.target_component = target_component; - mav_array_memcpy(packet.data, data, sizeof(int8_t)*48); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_flexifunction_buffer_function_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t func_count, uint16_t data_address, uint16_t data_size, const int8_t *data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, func_count); - _mav_put_uint16_t(buf, 4, data_address); - _mav_put_uint16_t(buf, 6, data_size); - _mav_put_uint8_t(buf, 8, target_system); - _mav_put_uint8_t(buf, 9, target_component); - _mav_put_int8_t_array(buf, 10, data, 48); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif -#else - mavlink_flexifunction_buffer_function_t *packet = (mavlink_flexifunction_buffer_function_t *)msgbuf; - packet->func_index = func_index; - packet->func_count = func_count; - packet->data_address = data_address; - packet->data_size = data_size; - packet->target_system = target_system; - packet->target_component = target_component; - mav_array_memcpy(packet->data, data, sizeof(int8_t)*48); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION UNPACKING - - -/** - * @brief Get field target_system from flexifunction_buffer_function message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_buffer_function_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field target_component from flexifunction_buffer_function message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_buffer_function_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 9); -} - -/** - * @brief Get field func_index from flexifunction_buffer_function message - * - * @return Function index - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_func_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field func_count from flexifunction_buffer_function message - * - * @return Total count of functions - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_func_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field data_address from flexifunction_buffer_function message - * - * @return Address in the flexifunction data, Set to 0xFFFF to use address in target memory - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_data_address(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field data_size from flexifunction_buffer_function message - * - * @return Size of the - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_data_size(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field data from flexifunction_buffer_function message - * - * @return Settings data - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_data(const mavlink_message_t* msg, int8_t *data) -{ - return _MAV_RETURN_int8_t_array(msg, data, 48, 10); -} - -/** - * @brief Decode a flexifunction_buffer_function message into a struct - * - * @param msg The message to decode - * @param flexifunction_buffer_function C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_buffer_function_decode(const mavlink_message_t* msg, mavlink_flexifunction_buffer_function_t* flexifunction_buffer_function) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_buffer_function->func_index = mavlink_msg_flexifunction_buffer_function_get_func_index(msg); - flexifunction_buffer_function->func_count = mavlink_msg_flexifunction_buffer_function_get_func_count(msg); - flexifunction_buffer_function->data_address = mavlink_msg_flexifunction_buffer_function_get_data_address(msg); - flexifunction_buffer_function->data_size = mavlink_msg_flexifunction_buffer_function_get_data_size(msg); - flexifunction_buffer_function->target_system = mavlink_msg_flexifunction_buffer_function_get_target_system(msg); - flexifunction_buffer_function->target_component = mavlink_msg_flexifunction_buffer_function_get_target_component(msg); - mavlink_msg_flexifunction_buffer_function_get_data(msg, flexifunction_buffer_function->data); -#else - memcpy(flexifunction_buffer_function, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h deleted file mode 100644 index 06631b40e..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION_ACK PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK 153 - -typedef struct __mavlink_flexifunction_buffer_function_ack_t -{ - uint16_t func_index; ///< Function index - uint16_t result; ///< result of acknowledge, 0=fail, 1=good - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_flexifunction_buffer_function_ack_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN 6 -#define MAVLINK_MSG_ID_153_LEN 6 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC 109 -#define MAVLINK_MSG_ID_153_CRC 109 - - - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_BUFFER_FUNCTION_ACK { \ - "FLEXIFUNCTION_BUFFER_FUNCTION_ACK", \ - 4, \ - { { "func_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_flexifunction_buffer_function_ack_t, func_index) }, \ - { "result", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_flexifunction_buffer_function_ack_t, result) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_flexifunction_buffer_function_ack_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_flexifunction_buffer_function_ack_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_buffer_function_ack message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param func_index Function index - * @param result result of acknowledge, 0=fail, 1=good - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN]; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, result); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#else - mavlink_flexifunction_buffer_function_ack_t packet; - packet.func_index = func_index; - packet.result = result; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_buffer_function_ack message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param func_index Function index - * @param result result of acknowledge, 0=fail, 1=good - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t func_index,uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN]; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, result); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#else - mavlink_flexifunction_buffer_function_ack_t packet; - packet.func_index = func_index; - packet.result = result; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_buffer_function_ack struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_buffer_function_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_ack_t* flexifunction_buffer_function_ack) -{ - return mavlink_msg_flexifunction_buffer_function_ack_pack(system_id, component_id, msg, flexifunction_buffer_function_ack->target_system, flexifunction_buffer_function_ack->target_component, flexifunction_buffer_function_ack->func_index, flexifunction_buffer_function_ack->result); -} - -/** - * @brief Encode a flexifunction_buffer_function_ack struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_buffer_function_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_ack_t* flexifunction_buffer_function_ack) -{ - return mavlink_msg_flexifunction_buffer_function_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_buffer_function_ack->target_system, flexifunction_buffer_function_ack->target_component, flexifunction_buffer_function_ack->func_index, flexifunction_buffer_function_ack->result); -} - -/** - * @brief Send a flexifunction_buffer_function_ack message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param func_index Function index - * @param result result of acknowledge, 0=fail, 1=good - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_buffer_function_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN]; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, result); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif -#else - mavlink_flexifunction_buffer_function_ack_t packet; - packet.func_index = func_index; - packet.result = result; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_flexifunction_buffer_function_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, func_index); - _mav_put_uint16_t(buf, 2, result); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif -#else - mavlink_flexifunction_buffer_function_ack_t *packet = (mavlink_flexifunction_buffer_function_ack_t *)msgbuf; - packet->func_index = func_index; - packet->result = result; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION_ACK UNPACKING - - -/** - * @brief Get field target_system from flexifunction_buffer_function_ack message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_buffer_function_ack_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field target_component from flexifunction_buffer_function_ack message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_buffer_function_ack_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field func_index from flexifunction_buffer_function_ack message - * - * @return Function index - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_get_func_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field result from flexifunction_buffer_function_ack message - * - * @return result of acknowledge, 0=fail, 1=good - */ -static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_get_result(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Decode a flexifunction_buffer_function_ack message into a struct - * - * @param msg The message to decode - * @param flexifunction_buffer_function_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_buffer_function_ack_decode(const mavlink_message_t* msg, mavlink_flexifunction_buffer_function_ack_t* flexifunction_buffer_function_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_buffer_function_ack->func_index = mavlink_msg_flexifunction_buffer_function_ack_get_func_index(msg); - flexifunction_buffer_function_ack->result = mavlink_msg_flexifunction_buffer_function_ack_get_result(msg); - flexifunction_buffer_function_ack->target_system = mavlink_msg_flexifunction_buffer_function_ack_get_target_system(msg); - flexifunction_buffer_function_ack->target_component = mavlink_msg_flexifunction_buffer_function_ack_get_target_component(msg); -#else - memcpy(flexifunction_buffer_function_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h deleted file mode 100644 index ac8366eef..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h +++ /dev/null @@ -1,257 +0,0 @@ -// MESSAGE FLEXIFUNCTION_COMMAND PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND 157 - -typedef struct __mavlink_flexifunction_command_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t command_type; ///< Flexifunction command type -} mavlink_flexifunction_command_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN 3 -#define MAVLINK_MSG_ID_157_LEN 3 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC 133 -#define MAVLINK_MSG_ID_157_CRC 133 - - - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_COMMAND { \ - "FLEXIFUNCTION_COMMAND", \ - 3, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_flexifunction_command_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_flexifunction_command_t, target_component) }, \ - { "command_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_flexifunction_command_t, command_type) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_command message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param command_type Flexifunction command type - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t command_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, command_type); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#else - mavlink_flexifunction_command_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.command_type = command_type; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_command message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param command_type Flexifunction command type - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_command_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t command_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, command_type); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#else - mavlink_flexifunction_command_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.command_type = command_type; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_command struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_command C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_command_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_command_t* flexifunction_command) -{ - return mavlink_msg_flexifunction_command_pack(system_id, component_id, msg, flexifunction_command->target_system, flexifunction_command->target_component, flexifunction_command->command_type); -} - -/** - * @brief Encode a flexifunction_command struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_command C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_command_t* flexifunction_command) -{ - return mavlink_msg_flexifunction_command_pack_chan(system_id, component_id, chan, msg, flexifunction_command->target_system, flexifunction_command->target_component, flexifunction_command->command_type); -} - -/** - * @brief Send a flexifunction_command message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param command_type Flexifunction command type - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, command_type); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif -#else - mavlink_flexifunction_command_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.command_type = command_type; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_flexifunction_command_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, command_type); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif -#else - mavlink_flexifunction_command_t *packet = (mavlink_flexifunction_command_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - packet->command_type = command_type; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FLEXIFUNCTION_COMMAND UNPACKING - - -/** - * @brief Get field target_system from flexifunction_command message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_command_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from flexifunction_command message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_command_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field command_type from flexifunction_command message - * - * @return Flexifunction command type - */ -static inline uint8_t mavlink_msg_flexifunction_command_get_command_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a flexifunction_command message into a struct - * - * @param msg The message to decode - * @param flexifunction_command C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_command_decode(const mavlink_message_t* msg, mavlink_flexifunction_command_t* flexifunction_command) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_command->target_system = mavlink_msg_flexifunction_command_get_target_system(msg); - flexifunction_command->target_component = mavlink_msg_flexifunction_command_get_target_component(msg); - flexifunction_command->command_type = mavlink_msg_flexifunction_command_get_command_type(msg); -#else - memcpy(flexifunction_command, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h deleted file mode 100644 index c88e2dc39..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE FLEXIFUNCTION_COMMAND_ACK PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK 158 - -typedef struct __mavlink_flexifunction_command_ack_t -{ - uint16_t command_type; ///< Command acknowledged - uint16_t result; ///< result of acknowledge -} mavlink_flexifunction_command_ack_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN 4 -#define MAVLINK_MSG_ID_158_LEN 4 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC 208 -#define MAVLINK_MSG_ID_158_CRC 208 - - - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_COMMAND_ACK { \ - "FLEXIFUNCTION_COMMAND_ACK", \ - 2, \ - { { "command_type", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_flexifunction_command_ack_t, command_type) }, \ - { "result", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_flexifunction_command_ack_t, result) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_command_ack message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param command_type Command acknowledged - * @param result result of acknowledge - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t command_type, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN]; - _mav_put_uint16_t(buf, 0, command_type); - _mav_put_uint16_t(buf, 2, result); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#else - mavlink_flexifunction_command_ack_t packet; - packet.command_type = command_type; - packet.result = result; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_command_ack message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param command_type Command acknowledged - * @param result result of acknowledge - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t command_type,uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN]; - _mav_put_uint16_t(buf, 0, command_type); - _mav_put_uint16_t(buf, 2, result); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#else - mavlink_flexifunction_command_ack_t packet; - packet.command_type = command_type; - packet.result = result; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_command_ack struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_command_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_command_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_command_ack_t* flexifunction_command_ack) -{ - return mavlink_msg_flexifunction_command_ack_pack(system_id, component_id, msg, flexifunction_command_ack->command_type, flexifunction_command_ack->result); -} - -/** - * @brief Encode a flexifunction_command_ack struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_command_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_command_ack_t* flexifunction_command_ack) -{ - return mavlink_msg_flexifunction_command_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_command_ack->command_type, flexifunction_command_ack->result); -} - -/** - * @brief Send a flexifunction_command_ack message - * @param chan MAVLink channel to send the message - * - * @param command_type Command acknowledged - * @param result result of acknowledge - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_command_ack_send(mavlink_channel_t chan, uint16_t command_type, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN]; - _mav_put_uint16_t(buf, 0, command_type); - _mav_put_uint16_t(buf, 2, result); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif -#else - mavlink_flexifunction_command_ack_t packet; - packet.command_type = command_type; - packet.result = result; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_flexifunction_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t command_type, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, command_type); - _mav_put_uint16_t(buf, 2, result); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif -#else - mavlink_flexifunction_command_ack_t *packet = (mavlink_flexifunction_command_ack_t *)msgbuf; - packet->command_type = command_type; - packet->result = result; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FLEXIFUNCTION_COMMAND_ACK UNPACKING - - -/** - * @brief Get field command_type from flexifunction_command_ack message - * - * @return Command acknowledged - */ -static inline uint16_t mavlink_msg_flexifunction_command_ack_get_command_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field result from flexifunction_command_ack message - * - * @return result of acknowledge - */ -static inline uint16_t mavlink_msg_flexifunction_command_ack_get_result(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Decode a flexifunction_command_ack message into a struct - * - * @param msg The message to decode - * @param flexifunction_command_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_command_ack_decode(const mavlink_message_t* msg, mavlink_flexifunction_command_ack_t* flexifunction_command_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_command_ack->command_type = mavlink_msg_flexifunction_command_ack_get_command_type(msg); - flexifunction_command_ack->result = mavlink_msg_flexifunction_command_ack_get_result(msg); -#else - memcpy(flexifunction_command_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h deleted file mode 100644 index 619f810c5..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h +++ /dev/null @@ -1,321 +0,0 @@ -// MESSAGE FLEXIFUNCTION_DIRECTORY PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY 155 - -typedef struct __mavlink_flexifunction_directory_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t directory_type; ///< 0=inputs, 1=outputs - uint8_t start_index; ///< index of first directory entry to write - uint8_t count; ///< count of directory entries to write - int8_t directory_data[48]; ///< Settings data -} mavlink_flexifunction_directory_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN 53 -#define MAVLINK_MSG_ID_155_LEN 53 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC 12 -#define MAVLINK_MSG_ID_155_CRC 12 - -#define MAVLINK_MSG_FLEXIFUNCTION_DIRECTORY_FIELD_DIRECTORY_DATA_LEN 48 - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_DIRECTORY { \ - "FLEXIFUNCTION_DIRECTORY", \ - 6, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_flexifunction_directory_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_flexifunction_directory_t, target_component) }, \ - { "directory_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_flexifunction_directory_t, directory_type) }, \ - { "start_index", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_flexifunction_directory_t, start_index) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_flexifunction_directory_t, count) }, \ - { "directory_data", NULL, MAVLINK_TYPE_INT8_T, 48, 5, offsetof(mavlink_flexifunction_directory_t, directory_data) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_directory message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param directory_type 0=inputs, 1=outputs - * @param start_index index of first directory entry to write - * @param count count of directory entries to write - * @param directory_data Settings data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_directory_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, const int8_t *directory_data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, directory_type); - _mav_put_uint8_t(buf, 3, start_index); - _mav_put_uint8_t(buf, 4, count); - _mav_put_int8_t_array(buf, 5, directory_data, 48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#else - mavlink_flexifunction_directory_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.directory_type = directory_type; - packet.start_index = start_index; - packet.count = count; - mav_array_memcpy(packet.directory_data, directory_data, sizeof(int8_t)*48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_directory message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param directory_type 0=inputs, 1=outputs - * @param start_index index of first directory entry to write - * @param count count of directory entries to write - * @param directory_data Settings data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_directory_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t directory_type,uint8_t start_index,uint8_t count,const int8_t *directory_data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, directory_type); - _mav_put_uint8_t(buf, 3, start_index); - _mav_put_uint8_t(buf, 4, count); - _mav_put_int8_t_array(buf, 5, directory_data, 48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#else - mavlink_flexifunction_directory_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.directory_type = directory_type; - packet.start_index = start_index; - packet.count = count; - mav_array_memcpy(packet.directory_data, directory_data, sizeof(int8_t)*48); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_directory struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_directory C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_directory_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_directory_t* flexifunction_directory) -{ - return mavlink_msg_flexifunction_directory_pack(system_id, component_id, msg, flexifunction_directory->target_system, flexifunction_directory->target_component, flexifunction_directory->directory_type, flexifunction_directory->start_index, flexifunction_directory->count, flexifunction_directory->directory_data); -} - -/** - * @brief Encode a flexifunction_directory struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_directory C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_directory_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_directory_t* flexifunction_directory) -{ - return mavlink_msg_flexifunction_directory_pack_chan(system_id, component_id, chan, msg, flexifunction_directory->target_system, flexifunction_directory->target_component, flexifunction_directory->directory_type, flexifunction_directory->start_index, flexifunction_directory->count, flexifunction_directory->directory_data); -} - -/** - * @brief Send a flexifunction_directory message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param directory_type 0=inputs, 1=outputs - * @param start_index index of first directory entry to write - * @param count count of directory entries to write - * @param directory_data Settings data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_directory_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, const int8_t *directory_data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, directory_type); - _mav_put_uint8_t(buf, 3, start_index); - _mav_put_uint8_t(buf, 4, count); - _mav_put_int8_t_array(buf, 5, directory_data, 48); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif -#else - mavlink_flexifunction_directory_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.directory_type = directory_type; - packet.start_index = start_index; - packet.count = count; - mav_array_memcpy(packet.directory_data, directory_data, sizeof(int8_t)*48); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_flexifunction_directory_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, const int8_t *directory_data) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, directory_type); - _mav_put_uint8_t(buf, 3, start_index); - _mav_put_uint8_t(buf, 4, count); - _mav_put_int8_t_array(buf, 5, directory_data, 48); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif -#else - mavlink_flexifunction_directory_t *packet = (mavlink_flexifunction_directory_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - packet->directory_type = directory_type; - packet->start_index = start_index; - packet->count = count; - mav_array_memcpy(packet->directory_data, directory_data, sizeof(int8_t)*48); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FLEXIFUNCTION_DIRECTORY UNPACKING - - -/** - * @brief Get field target_system from flexifunction_directory message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_directory_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from flexifunction_directory message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_directory_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field directory_type from flexifunction_directory message - * - * @return 0=inputs, 1=outputs - */ -static inline uint8_t mavlink_msg_flexifunction_directory_get_directory_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field start_index from flexifunction_directory message - * - * @return index of first directory entry to write - */ -static inline uint8_t mavlink_msg_flexifunction_directory_get_start_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field count from flexifunction_directory message - * - * @return count of directory entries to write - */ -static inline uint8_t mavlink_msg_flexifunction_directory_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field directory_data from flexifunction_directory message - * - * @return Settings data - */ -static inline uint16_t mavlink_msg_flexifunction_directory_get_directory_data(const mavlink_message_t* msg, int8_t *directory_data) -{ - return _MAV_RETURN_int8_t_array(msg, directory_data, 48, 5); -} - -/** - * @brief Decode a flexifunction_directory message into a struct - * - * @param msg The message to decode - * @param flexifunction_directory C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_directory_decode(const mavlink_message_t* msg, mavlink_flexifunction_directory_t* flexifunction_directory) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_directory->target_system = mavlink_msg_flexifunction_directory_get_target_system(msg); - flexifunction_directory->target_component = mavlink_msg_flexifunction_directory_get_target_component(msg); - flexifunction_directory->directory_type = mavlink_msg_flexifunction_directory_get_directory_type(msg); - flexifunction_directory->start_index = mavlink_msg_flexifunction_directory_get_start_index(msg); - flexifunction_directory->count = mavlink_msg_flexifunction_directory_get_count(msg); - mavlink_msg_flexifunction_directory_get_directory_data(msg, flexifunction_directory->directory_data); -#else - memcpy(flexifunction_directory, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h deleted file mode 100644 index 0287183d8..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE FLEXIFUNCTION_DIRECTORY_ACK PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK 156 - -typedef struct __mavlink_flexifunction_directory_ack_t -{ - uint16_t result; ///< result of acknowledge, 0=fail, 1=good - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t directory_type; ///< 0=inputs, 1=outputs - uint8_t start_index; ///< index of first directory entry to write - uint8_t count; ///< count of directory entries to write -} mavlink_flexifunction_directory_ack_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN 7 -#define MAVLINK_MSG_ID_156_LEN 7 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC 218 -#define MAVLINK_MSG_ID_156_CRC 218 - - - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_DIRECTORY_ACK { \ - "FLEXIFUNCTION_DIRECTORY_ACK", \ - 6, \ - { { "result", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_flexifunction_directory_ack_t, result) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_flexifunction_directory_ack_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_flexifunction_directory_ack_t, target_component) }, \ - { "directory_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_flexifunction_directory_ack_t, directory_type) }, \ - { "start_index", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_flexifunction_directory_ack_t, start_index) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_flexifunction_directory_ack_t, count) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_directory_ack message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param directory_type 0=inputs, 1=outputs - * @param start_index index of first directory entry to write - * @param count count of directory entries to write - * @param result result of acknowledge, 0=fail, 1=good - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN]; - _mav_put_uint16_t(buf, 0, result); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_uint8_t(buf, 4, directory_type); - _mav_put_uint8_t(buf, 5, start_index); - _mav_put_uint8_t(buf, 6, count); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#else - mavlink_flexifunction_directory_ack_t packet; - packet.result = result; - packet.target_system = target_system; - packet.target_component = target_component; - packet.directory_type = directory_type; - packet.start_index = start_index; - packet.count = count; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_directory_ack message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param directory_type 0=inputs, 1=outputs - * @param start_index index of first directory entry to write - * @param count count of directory entries to write - * @param result result of acknowledge, 0=fail, 1=good - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t directory_type,uint8_t start_index,uint8_t count,uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN]; - _mav_put_uint16_t(buf, 0, result); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_uint8_t(buf, 4, directory_type); - _mav_put_uint8_t(buf, 5, start_index); - _mav_put_uint8_t(buf, 6, count); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#else - mavlink_flexifunction_directory_ack_t packet; - packet.result = result; - packet.target_system = target_system; - packet.target_component = target_component; - packet.directory_type = directory_type; - packet.start_index = start_index; - packet.count = count; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_directory_ack struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_directory_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_directory_ack_t* flexifunction_directory_ack) -{ - return mavlink_msg_flexifunction_directory_ack_pack(system_id, component_id, msg, flexifunction_directory_ack->target_system, flexifunction_directory_ack->target_component, flexifunction_directory_ack->directory_type, flexifunction_directory_ack->start_index, flexifunction_directory_ack->count, flexifunction_directory_ack->result); -} - -/** - * @brief Encode a flexifunction_directory_ack struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_directory_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_directory_ack_t* flexifunction_directory_ack) -{ - return mavlink_msg_flexifunction_directory_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_directory_ack->target_system, flexifunction_directory_ack->target_component, flexifunction_directory_ack->directory_type, flexifunction_directory_ack->start_index, flexifunction_directory_ack->count, flexifunction_directory_ack->result); -} - -/** - * @brief Send a flexifunction_directory_ack message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param directory_type 0=inputs, 1=outputs - * @param start_index index of first directory entry to write - * @param count count of directory entries to write - * @param result result of acknowledge, 0=fail, 1=good - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_directory_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN]; - _mav_put_uint16_t(buf, 0, result); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_uint8_t(buf, 4, directory_type); - _mav_put_uint8_t(buf, 5, start_index); - _mav_put_uint8_t(buf, 6, count); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif -#else - mavlink_flexifunction_directory_ack_t packet; - packet.result = result; - packet.target_system = target_system; - packet.target_component = target_component; - packet.directory_type = directory_type; - packet.start_index = start_index; - packet.count = count; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_flexifunction_directory_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, uint16_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, result); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - _mav_put_uint8_t(buf, 4, directory_type); - _mav_put_uint8_t(buf, 5, start_index); - _mav_put_uint8_t(buf, 6, count); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif -#else - mavlink_flexifunction_directory_ack_t *packet = (mavlink_flexifunction_directory_ack_t *)msgbuf; - packet->result = result; - packet->target_system = target_system; - packet->target_component = target_component; - packet->directory_type = directory_type; - packet->start_index = start_index; - packet->count = count; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FLEXIFUNCTION_DIRECTORY_ACK UNPACKING - - -/** - * @brief Get field target_system from flexifunction_directory_ack message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from flexifunction_directory_ack message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field directory_type from flexifunction_directory_ack message - * - * @return 0=inputs, 1=outputs - */ -static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_directory_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field start_index from flexifunction_directory_ack message - * - * @return index of first directory entry to write - */ -static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_start_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field count from flexifunction_directory_ack message - * - * @return count of directory entries to write - */ -static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field result from flexifunction_directory_ack message - * - * @return result of acknowledge, 0=fail, 1=good - */ -static inline uint16_t mavlink_msg_flexifunction_directory_ack_get_result(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a flexifunction_directory_ack message into a struct - * - * @param msg The message to decode - * @param flexifunction_directory_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_directory_ack_decode(const mavlink_message_t* msg, mavlink_flexifunction_directory_ack_t* flexifunction_directory_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_directory_ack->result = mavlink_msg_flexifunction_directory_ack_get_result(msg); - flexifunction_directory_ack->target_system = mavlink_msg_flexifunction_directory_ack_get_target_system(msg); - flexifunction_directory_ack->target_component = mavlink_msg_flexifunction_directory_ack_get_target_component(msg); - flexifunction_directory_ack->directory_type = mavlink_msg_flexifunction_directory_ack_get_directory_type(msg); - flexifunction_directory_ack->start_index = mavlink_msg_flexifunction_directory_ack_get_start_index(msg); - flexifunction_directory_ack->count = mavlink_msg_flexifunction_directory_ack_get_count(msg); -#else - memcpy(flexifunction_directory_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h deleted file mode 100644 index 570782e79..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE FLEXIFUNCTION_READ_REQ PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ 151 - -typedef struct __mavlink_flexifunction_read_req_t -{ - int16_t read_req_type; ///< Type of flexifunction data requested - int16_t data_index; ///< index into data where needed - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_flexifunction_read_req_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN 6 -#define MAVLINK_MSG_ID_151_LEN 6 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC 26 -#define MAVLINK_MSG_ID_151_CRC 26 - - - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_READ_REQ { \ - "FLEXIFUNCTION_READ_REQ", \ - 4, \ - { { "read_req_type", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_flexifunction_read_req_t, read_req_type) }, \ - { "data_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_flexifunction_read_req_t, data_index) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_flexifunction_read_req_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_flexifunction_read_req_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_read_req message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param read_req_type Type of flexifunction data requested - * @param data_index index into data where needed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_read_req_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int16_t read_req_type, int16_t data_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN]; - _mav_put_int16_t(buf, 0, read_req_type); - _mav_put_int16_t(buf, 2, data_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#else - mavlink_flexifunction_read_req_t packet; - packet.read_req_type = read_req_type; - packet.data_index = data_index; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_read_req message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param read_req_type Type of flexifunction data requested - * @param data_index index into data where needed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_read_req_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int16_t read_req_type,int16_t data_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN]; - _mav_put_int16_t(buf, 0, read_req_type); - _mav_put_int16_t(buf, 2, data_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#else - mavlink_flexifunction_read_req_t packet; - packet.read_req_type = read_req_type; - packet.data_index = data_index; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_read_req struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_read_req C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_read_req_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_read_req_t* flexifunction_read_req) -{ - return mavlink_msg_flexifunction_read_req_pack(system_id, component_id, msg, flexifunction_read_req->target_system, flexifunction_read_req->target_component, flexifunction_read_req->read_req_type, flexifunction_read_req->data_index); -} - -/** - * @brief Encode a flexifunction_read_req struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_read_req C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_read_req_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_read_req_t* flexifunction_read_req) -{ - return mavlink_msg_flexifunction_read_req_pack_chan(system_id, component_id, chan, msg, flexifunction_read_req->target_system, flexifunction_read_req->target_component, flexifunction_read_req->read_req_type, flexifunction_read_req->data_index); -} - -/** - * @brief Send a flexifunction_read_req message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param read_req_type Type of flexifunction data requested - * @param data_index index into data where needed - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_read_req_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t read_req_type, int16_t data_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN]; - _mav_put_int16_t(buf, 0, read_req_type); - _mav_put_int16_t(buf, 2, data_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif -#else - mavlink_flexifunction_read_req_t packet; - packet.read_req_type = read_req_type; - packet.data_index = data_index; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_flexifunction_read_req_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t read_req_type, int16_t data_index) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int16_t(buf, 0, read_req_type); - _mav_put_int16_t(buf, 2, data_index); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif -#else - mavlink_flexifunction_read_req_t *packet = (mavlink_flexifunction_read_req_t *)msgbuf; - packet->read_req_type = read_req_type; - packet->data_index = data_index; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FLEXIFUNCTION_READ_REQ UNPACKING - - -/** - * @brief Get field target_system from flexifunction_read_req message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_read_req_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field target_component from flexifunction_read_req message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_read_req_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field read_req_type from flexifunction_read_req message - * - * @return Type of flexifunction data requested - */ -static inline int16_t mavlink_msg_flexifunction_read_req_get_read_req_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 0); -} - -/** - * @brief Get field data_index from flexifunction_read_req message - * - * @return index into data where needed - */ -static inline int16_t mavlink_msg_flexifunction_read_req_get_data_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 2); -} - -/** - * @brief Decode a flexifunction_read_req message into a struct - * - * @param msg The message to decode - * @param flexifunction_read_req C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_read_req_decode(const mavlink_message_t* msg, mavlink_flexifunction_read_req_t* flexifunction_read_req) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_read_req->read_req_type = mavlink_msg_flexifunction_read_req_get_read_req_type(msg); - flexifunction_read_req->data_index = mavlink_msg_flexifunction_read_req_get_data_index(msg); - flexifunction_read_req->target_system = mavlink_msg_flexifunction_read_req_get_target_system(msg); - flexifunction_read_req->target_component = mavlink_msg_flexifunction_read_req_get_target_component(msg); -#else - memcpy(flexifunction_read_req, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h deleted file mode 100644 index c10000640..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE FLEXIFUNCTION_SET PACKING - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_SET 150 - -typedef struct __mavlink_flexifunction_set_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_flexifunction_set_t; - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN 2 -#define MAVLINK_MSG_ID_150_LEN 2 - -#define MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC 181 -#define MAVLINK_MSG_ID_150_CRC 181 - - - -#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_SET { \ - "FLEXIFUNCTION_SET", \ - 2, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_flexifunction_set_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_flexifunction_set_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a flexifunction_set message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#else - mavlink_flexifunction_set_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_SET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif -} - -/** - * @brief Pack a flexifunction_set message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_flexifunction_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#else - mavlink_flexifunction_set_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_SET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif -} - -/** - * @brief Encode a flexifunction_set struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param flexifunction_set C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_set_t* flexifunction_set) -{ - return mavlink_msg_flexifunction_set_pack(system_id, component_id, msg, flexifunction_set->target_system, flexifunction_set->target_component); -} - -/** - * @brief Encode a flexifunction_set struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param flexifunction_set C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_flexifunction_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_set_t* flexifunction_set) -{ - return mavlink_msg_flexifunction_set_pack_chan(system_id, component_id, chan, msg, flexifunction_set->target_system, flexifunction_set->target_component); -} - -/** - * @brief Send a flexifunction_set message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_flexifunction_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif -#else - mavlink_flexifunction_set_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, (const char *)&packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_flexifunction_set_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif -#else - mavlink_flexifunction_set_t *packet = (mavlink_flexifunction_set_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FLEXIFUNCTION_SET UNPACKING - - -/** - * @brief Get field target_system from flexifunction_set message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_flexifunction_set_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from flexifunction_set message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_flexifunction_set_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a flexifunction_set message into a struct - * - * @param msg The message to decode - * @param flexifunction_set C-struct to decode the message contents into - */ -static inline void mavlink_msg_flexifunction_set_decode(const mavlink_message_t* msg, mavlink_flexifunction_set_t* flexifunction_set) -{ -#if MAVLINK_NEED_BYTE_SWAP - flexifunction_set->target_system = mavlink_msg_flexifunction_set_get_target_system(msg); - flexifunction_set->target_component = mavlink_msg_flexifunction_set_get_target_component(msg); -#else - memcpy(flexifunction_set, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h deleted file mode 100644 index bbb753a06..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F13 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13 177 - -typedef struct __mavlink_serial_udb_extra_f13_t -{ - int32_t sue_lat_origin; ///< Serial UDB Extra MP Origin Latitude - int32_t sue_lon_origin; ///< Serial UDB Extra MP Origin Longitude - int32_t sue_alt_origin; ///< Serial UDB Extra MP Origin Altitude Above Sea Level - int16_t sue_week_no; ///< Serial UDB Extra GPS Week Number -} mavlink_serial_udb_extra_f13_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN 14 -#define MAVLINK_MSG_ID_177_LEN 14 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC 249 -#define MAVLINK_MSG_ID_177_CRC 249 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F13 { \ - "SERIAL_UDB_EXTRA_F13", \ - 4, \ - { { "sue_lat_origin", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_serial_udb_extra_f13_t, sue_lat_origin) }, \ - { "sue_lon_origin", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_serial_udb_extra_f13_t, sue_lon_origin) }, \ - { "sue_alt_origin", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_serial_udb_extra_f13_t, sue_alt_origin) }, \ - { "sue_week_no", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_serial_udb_extra_f13_t, sue_week_no) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f13 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_week_no Serial UDB Extra GPS Week Number - * @param sue_lat_origin Serial UDB Extra MP Origin Latitude - * @param sue_lon_origin Serial UDB Extra MP Origin Longitude - * @param sue_alt_origin Serial UDB Extra MP Origin Altitude Above Sea Level - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int16_t sue_week_no, int32_t sue_lat_origin, int32_t sue_lon_origin, int32_t sue_alt_origin) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN]; - _mav_put_int32_t(buf, 0, sue_lat_origin); - _mav_put_int32_t(buf, 4, sue_lon_origin); - _mav_put_int32_t(buf, 8, sue_alt_origin); - _mav_put_int16_t(buf, 12, sue_week_no); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#else - mavlink_serial_udb_extra_f13_t packet; - packet.sue_lat_origin = sue_lat_origin; - packet.sue_lon_origin = sue_lon_origin; - packet.sue_alt_origin = sue_alt_origin; - packet.sue_week_no = sue_week_no; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f13 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_week_no Serial UDB Extra GPS Week Number - * @param sue_lat_origin Serial UDB Extra MP Origin Latitude - * @param sue_lon_origin Serial UDB Extra MP Origin Longitude - * @param sue_alt_origin Serial UDB Extra MP Origin Altitude Above Sea Level - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - int16_t sue_week_no,int32_t sue_lat_origin,int32_t sue_lon_origin,int32_t sue_alt_origin) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN]; - _mav_put_int32_t(buf, 0, sue_lat_origin); - _mav_put_int32_t(buf, 4, sue_lon_origin); - _mav_put_int32_t(buf, 8, sue_alt_origin); - _mav_put_int16_t(buf, 12, sue_week_no); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#else - mavlink_serial_udb_extra_f13_t packet; - packet.sue_lat_origin = sue_lat_origin; - packet.sue_lon_origin = sue_lon_origin; - packet.sue_alt_origin = sue_alt_origin; - packet.sue_week_no = sue_week_no; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f13 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f13 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f13_t* serial_udb_extra_f13) -{ - return mavlink_msg_serial_udb_extra_f13_pack(system_id, component_id, msg, serial_udb_extra_f13->sue_week_no, serial_udb_extra_f13->sue_lat_origin, serial_udb_extra_f13->sue_lon_origin, serial_udb_extra_f13->sue_alt_origin); -} - -/** - * @brief Encode a serial_udb_extra_f13 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f13 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f13_t* serial_udb_extra_f13) -{ - return mavlink_msg_serial_udb_extra_f13_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f13->sue_week_no, serial_udb_extra_f13->sue_lat_origin, serial_udb_extra_f13->sue_lon_origin, serial_udb_extra_f13->sue_alt_origin); -} - -/** - * @brief Send a serial_udb_extra_f13 message - * @param chan MAVLink channel to send the message - * - * @param sue_week_no Serial UDB Extra GPS Week Number - * @param sue_lat_origin Serial UDB Extra MP Origin Latitude - * @param sue_lon_origin Serial UDB Extra MP Origin Longitude - * @param sue_alt_origin Serial UDB Extra MP Origin Altitude Above Sea Level - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f13_send(mavlink_channel_t chan, int16_t sue_week_no, int32_t sue_lat_origin, int32_t sue_lon_origin, int32_t sue_alt_origin) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN]; - _mav_put_int32_t(buf, 0, sue_lat_origin); - _mav_put_int32_t(buf, 4, sue_lon_origin); - _mav_put_int32_t(buf, 8, sue_alt_origin); - _mav_put_int16_t(buf, 12, sue_week_no); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif -#else - mavlink_serial_udb_extra_f13_t packet; - packet.sue_lat_origin = sue_lat_origin; - packet.sue_lon_origin = sue_lon_origin; - packet.sue_alt_origin = sue_alt_origin; - packet.sue_week_no = sue_week_no; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_serial_udb_extra_f13_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int16_t sue_week_no, int32_t sue_lat_origin, int32_t sue_lon_origin, int32_t sue_alt_origin) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, sue_lat_origin); - _mav_put_int32_t(buf, 4, sue_lon_origin); - _mav_put_int32_t(buf, 8, sue_alt_origin); - _mav_put_int16_t(buf, 12, sue_week_no); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif -#else - mavlink_serial_udb_extra_f13_t *packet = (mavlink_serial_udb_extra_f13_t *)msgbuf; - packet->sue_lat_origin = sue_lat_origin; - packet->sue_lon_origin = sue_lon_origin; - packet->sue_alt_origin = sue_alt_origin; - packet->sue_week_no = sue_week_no; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F13 UNPACKING - - -/** - * @brief Get field sue_week_no from serial_udb_extra_f13 message - * - * @return Serial UDB Extra GPS Week Number - */ -static inline int16_t mavlink_msg_serial_udb_extra_f13_get_sue_week_no(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Get field sue_lat_origin from serial_udb_extra_f13 message - * - * @return Serial UDB Extra MP Origin Latitude - */ -static inline int32_t mavlink_msg_serial_udb_extra_f13_get_sue_lat_origin(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field sue_lon_origin from serial_udb_extra_f13 message - * - * @return Serial UDB Extra MP Origin Longitude - */ -static inline int32_t mavlink_msg_serial_udb_extra_f13_get_sue_lon_origin(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field sue_alt_origin from serial_udb_extra_f13 message - * - * @return Serial UDB Extra MP Origin Altitude Above Sea Level - */ -static inline int32_t mavlink_msg_serial_udb_extra_f13_get_sue_alt_origin(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Decode a serial_udb_extra_f13 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f13 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f13_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f13_t* serial_udb_extra_f13) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f13->sue_lat_origin = mavlink_msg_serial_udb_extra_f13_get_sue_lat_origin(msg); - serial_udb_extra_f13->sue_lon_origin = mavlink_msg_serial_udb_extra_f13_get_sue_lon_origin(msg); - serial_udb_extra_f13->sue_alt_origin = mavlink_msg_serial_udb_extra_f13_get_sue_alt_origin(msg); - serial_udb_extra_f13->sue_week_no = mavlink_msg_serial_udb_extra_f13_get_sue_week_no(msg); -#else - memcpy(serial_udb_extra_f13, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h deleted file mode 100644 index 112564d00..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h +++ /dev/null @@ -1,449 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F14 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14 178 - -typedef struct __mavlink_serial_udb_extra_f14_t -{ - uint32_t sue_TRAP_SOURCE; ///< Serial UDB Extra Type Program Address of Last Trap - int16_t sue_RCON; ///< Serial UDB Extra Reboot Regitster of DSPIC - int16_t sue_TRAP_FLAGS; ///< Serial UDB Extra Last dspic Trap Flags - int16_t sue_osc_fail_count; ///< Serial UDB Extra Number of Ocillator Failures - uint8_t sue_WIND_ESTIMATION; ///< Serial UDB Extra Wind Estimation Enabled - uint8_t sue_GPS_TYPE; ///< Serial UDB Extra Type of GPS Unit - uint8_t sue_DR; ///< Serial UDB Extra Dead Reckoning Enabled - uint8_t sue_BOARD_TYPE; ///< Serial UDB Extra Type of UDB Hardware - uint8_t sue_AIRFRAME; ///< Serial UDB Extra Type of Airframe - uint8_t sue_CLOCK_CONFIG; ///< Serial UDB Extra UDB Internal Clock Configuration - uint8_t sue_FLIGHT_PLAN_TYPE; ///< Serial UDB Extra Type of Flight Plan -} mavlink_serial_udb_extra_f14_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN 17 -#define MAVLINK_MSG_ID_178_LEN 17 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC 123 -#define MAVLINK_MSG_ID_178_CRC 123 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F14 { \ - "SERIAL_UDB_EXTRA_F14", \ - 11, \ - { { "sue_TRAP_SOURCE", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_udb_extra_f14_t, sue_TRAP_SOURCE) }, \ - { "sue_RCON", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_serial_udb_extra_f14_t, sue_RCON) }, \ - { "sue_TRAP_FLAGS", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_serial_udb_extra_f14_t, sue_TRAP_FLAGS) }, \ - { "sue_osc_fail_count", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_serial_udb_extra_f14_t, sue_osc_fail_count) }, \ - { "sue_WIND_ESTIMATION", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_serial_udb_extra_f14_t, sue_WIND_ESTIMATION) }, \ - { "sue_GPS_TYPE", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_serial_udb_extra_f14_t, sue_GPS_TYPE) }, \ - { "sue_DR", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_serial_udb_extra_f14_t, sue_DR) }, \ - { "sue_BOARD_TYPE", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_serial_udb_extra_f14_t, sue_BOARD_TYPE) }, \ - { "sue_AIRFRAME", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_serial_udb_extra_f14_t, sue_AIRFRAME) }, \ - { "sue_CLOCK_CONFIG", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_serial_udb_extra_f14_t, sue_CLOCK_CONFIG) }, \ - { "sue_FLIGHT_PLAN_TYPE", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_serial_udb_extra_f14_t, sue_FLIGHT_PLAN_TYPE) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f14 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_WIND_ESTIMATION Serial UDB Extra Wind Estimation Enabled - * @param sue_GPS_TYPE Serial UDB Extra Type of GPS Unit - * @param sue_DR Serial UDB Extra Dead Reckoning Enabled - * @param sue_BOARD_TYPE Serial UDB Extra Type of UDB Hardware - * @param sue_AIRFRAME Serial UDB Extra Type of Airframe - * @param sue_RCON Serial UDB Extra Reboot Regitster of DSPIC - * @param sue_TRAP_FLAGS Serial UDB Extra Last dspic Trap Flags - * @param sue_TRAP_SOURCE Serial UDB Extra Type Program Address of Last Trap - * @param sue_osc_fail_count Serial UDB Extra Number of Ocillator Failures - * @param sue_CLOCK_CONFIG Serial UDB Extra UDB Internal Clock Configuration - * @param sue_FLIGHT_PLAN_TYPE Serial UDB Extra Type of Flight Plan - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t sue_WIND_ESTIMATION, uint8_t sue_GPS_TYPE, uint8_t sue_DR, uint8_t sue_BOARD_TYPE, uint8_t sue_AIRFRAME, int16_t sue_RCON, int16_t sue_TRAP_FLAGS, uint32_t sue_TRAP_SOURCE, int16_t sue_osc_fail_count, uint8_t sue_CLOCK_CONFIG, uint8_t sue_FLIGHT_PLAN_TYPE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN]; - _mav_put_uint32_t(buf, 0, sue_TRAP_SOURCE); - _mav_put_int16_t(buf, 4, sue_RCON); - _mav_put_int16_t(buf, 6, sue_TRAP_FLAGS); - _mav_put_int16_t(buf, 8, sue_osc_fail_count); - _mav_put_uint8_t(buf, 10, sue_WIND_ESTIMATION); - _mav_put_uint8_t(buf, 11, sue_GPS_TYPE); - _mav_put_uint8_t(buf, 12, sue_DR); - _mav_put_uint8_t(buf, 13, sue_BOARD_TYPE); - _mav_put_uint8_t(buf, 14, sue_AIRFRAME); - _mav_put_uint8_t(buf, 15, sue_CLOCK_CONFIG); - _mav_put_uint8_t(buf, 16, sue_FLIGHT_PLAN_TYPE); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#else - mavlink_serial_udb_extra_f14_t packet; - packet.sue_TRAP_SOURCE = sue_TRAP_SOURCE; - packet.sue_RCON = sue_RCON; - packet.sue_TRAP_FLAGS = sue_TRAP_FLAGS; - packet.sue_osc_fail_count = sue_osc_fail_count; - packet.sue_WIND_ESTIMATION = sue_WIND_ESTIMATION; - packet.sue_GPS_TYPE = sue_GPS_TYPE; - packet.sue_DR = sue_DR; - packet.sue_BOARD_TYPE = sue_BOARD_TYPE; - packet.sue_AIRFRAME = sue_AIRFRAME; - packet.sue_CLOCK_CONFIG = sue_CLOCK_CONFIG; - packet.sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f14 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_WIND_ESTIMATION Serial UDB Extra Wind Estimation Enabled - * @param sue_GPS_TYPE Serial UDB Extra Type of GPS Unit - * @param sue_DR Serial UDB Extra Dead Reckoning Enabled - * @param sue_BOARD_TYPE Serial UDB Extra Type of UDB Hardware - * @param sue_AIRFRAME Serial UDB Extra Type of Airframe - * @param sue_RCON Serial UDB Extra Reboot Regitster of DSPIC - * @param sue_TRAP_FLAGS Serial UDB Extra Last dspic Trap Flags - * @param sue_TRAP_SOURCE Serial UDB Extra Type Program Address of Last Trap - * @param sue_osc_fail_count Serial UDB Extra Number of Ocillator Failures - * @param sue_CLOCK_CONFIG Serial UDB Extra UDB Internal Clock Configuration - * @param sue_FLIGHT_PLAN_TYPE Serial UDB Extra Type of Flight Plan - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t sue_WIND_ESTIMATION,uint8_t sue_GPS_TYPE,uint8_t sue_DR,uint8_t sue_BOARD_TYPE,uint8_t sue_AIRFRAME,int16_t sue_RCON,int16_t sue_TRAP_FLAGS,uint32_t sue_TRAP_SOURCE,int16_t sue_osc_fail_count,uint8_t sue_CLOCK_CONFIG,uint8_t sue_FLIGHT_PLAN_TYPE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN]; - _mav_put_uint32_t(buf, 0, sue_TRAP_SOURCE); - _mav_put_int16_t(buf, 4, sue_RCON); - _mav_put_int16_t(buf, 6, sue_TRAP_FLAGS); - _mav_put_int16_t(buf, 8, sue_osc_fail_count); - _mav_put_uint8_t(buf, 10, sue_WIND_ESTIMATION); - _mav_put_uint8_t(buf, 11, sue_GPS_TYPE); - _mav_put_uint8_t(buf, 12, sue_DR); - _mav_put_uint8_t(buf, 13, sue_BOARD_TYPE); - _mav_put_uint8_t(buf, 14, sue_AIRFRAME); - _mav_put_uint8_t(buf, 15, sue_CLOCK_CONFIG); - _mav_put_uint8_t(buf, 16, sue_FLIGHT_PLAN_TYPE); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#else - mavlink_serial_udb_extra_f14_t packet; - packet.sue_TRAP_SOURCE = sue_TRAP_SOURCE; - packet.sue_RCON = sue_RCON; - packet.sue_TRAP_FLAGS = sue_TRAP_FLAGS; - packet.sue_osc_fail_count = sue_osc_fail_count; - packet.sue_WIND_ESTIMATION = sue_WIND_ESTIMATION; - packet.sue_GPS_TYPE = sue_GPS_TYPE; - packet.sue_DR = sue_DR; - packet.sue_BOARD_TYPE = sue_BOARD_TYPE; - packet.sue_AIRFRAME = sue_AIRFRAME; - packet.sue_CLOCK_CONFIG = sue_CLOCK_CONFIG; - packet.sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f14 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f14 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f14_t* serial_udb_extra_f14) -{ - return mavlink_msg_serial_udb_extra_f14_pack(system_id, component_id, msg, serial_udb_extra_f14->sue_WIND_ESTIMATION, serial_udb_extra_f14->sue_GPS_TYPE, serial_udb_extra_f14->sue_DR, serial_udb_extra_f14->sue_BOARD_TYPE, serial_udb_extra_f14->sue_AIRFRAME, serial_udb_extra_f14->sue_RCON, serial_udb_extra_f14->sue_TRAP_FLAGS, serial_udb_extra_f14->sue_TRAP_SOURCE, serial_udb_extra_f14->sue_osc_fail_count, serial_udb_extra_f14->sue_CLOCK_CONFIG, serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE); -} - -/** - * @brief Encode a serial_udb_extra_f14 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f14 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f14_t* serial_udb_extra_f14) -{ - return mavlink_msg_serial_udb_extra_f14_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f14->sue_WIND_ESTIMATION, serial_udb_extra_f14->sue_GPS_TYPE, serial_udb_extra_f14->sue_DR, serial_udb_extra_f14->sue_BOARD_TYPE, serial_udb_extra_f14->sue_AIRFRAME, serial_udb_extra_f14->sue_RCON, serial_udb_extra_f14->sue_TRAP_FLAGS, serial_udb_extra_f14->sue_TRAP_SOURCE, serial_udb_extra_f14->sue_osc_fail_count, serial_udb_extra_f14->sue_CLOCK_CONFIG, serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE); -} - -/** - * @brief Send a serial_udb_extra_f14 message - * @param chan MAVLink channel to send the message - * - * @param sue_WIND_ESTIMATION Serial UDB Extra Wind Estimation Enabled - * @param sue_GPS_TYPE Serial UDB Extra Type of GPS Unit - * @param sue_DR Serial UDB Extra Dead Reckoning Enabled - * @param sue_BOARD_TYPE Serial UDB Extra Type of UDB Hardware - * @param sue_AIRFRAME Serial UDB Extra Type of Airframe - * @param sue_RCON Serial UDB Extra Reboot Regitster of DSPIC - * @param sue_TRAP_FLAGS Serial UDB Extra Last dspic Trap Flags - * @param sue_TRAP_SOURCE Serial UDB Extra Type Program Address of Last Trap - * @param sue_osc_fail_count Serial UDB Extra Number of Ocillator Failures - * @param sue_CLOCK_CONFIG Serial UDB Extra UDB Internal Clock Configuration - * @param sue_FLIGHT_PLAN_TYPE Serial UDB Extra Type of Flight Plan - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f14_send(mavlink_channel_t chan, uint8_t sue_WIND_ESTIMATION, uint8_t sue_GPS_TYPE, uint8_t sue_DR, uint8_t sue_BOARD_TYPE, uint8_t sue_AIRFRAME, int16_t sue_RCON, int16_t sue_TRAP_FLAGS, uint32_t sue_TRAP_SOURCE, int16_t sue_osc_fail_count, uint8_t sue_CLOCK_CONFIG, uint8_t sue_FLIGHT_PLAN_TYPE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN]; - _mav_put_uint32_t(buf, 0, sue_TRAP_SOURCE); - _mav_put_int16_t(buf, 4, sue_RCON); - _mav_put_int16_t(buf, 6, sue_TRAP_FLAGS); - _mav_put_int16_t(buf, 8, sue_osc_fail_count); - _mav_put_uint8_t(buf, 10, sue_WIND_ESTIMATION); - _mav_put_uint8_t(buf, 11, sue_GPS_TYPE); - _mav_put_uint8_t(buf, 12, sue_DR); - _mav_put_uint8_t(buf, 13, sue_BOARD_TYPE); - _mav_put_uint8_t(buf, 14, sue_AIRFRAME); - _mav_put_uint8_t(buf, 15, sue_CLOCK_CONFIG); - _mav_put_uint8_t(buf, 16, sue_FLIGHT_PLAN_TYPE); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif -#else - mavlink_serial_udb_extra_f14_t packet; - packet.sue_TRAP_SOURCE = sue_TRAP_SOURCE; - packet.sue_RCON = sue_RCON; - packet.sue_TRAP_FLAGS = sue_TRAP_FLAGS; - packet.sue_osc_fail_count = sue_osc_fail_count; - packet.sue_WIND_ESTIMATION = sue_WIND_ESTIMATION; - packet.sue_GPS_TYPE = sue_GPS_TYPE; - packet.sue_DR = sue_DR; - packet.sue_BOARD_TYPE = sue_BOARD_TYPE; - packet.sue_AIRFRAME = sue_AIRFRAME; - packet.sue_CLOCK_CONFIG = sue_CLOCK_CONFIG; - packet.sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_serial_udb_extra_f14_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t sue_WIND_ESTIMATION, uint8_t sue_GPS_TYPE, uint8_t sue_DR, uint8_t sue_BOARD_TYPE, uint8_t sue_AIRFRAME, int16_t sue_RCON, int16_t sue_TRAP_FLAGS, uint32_t sue_TRAP_SOURCE, int16_t sue_osc_fail_count, uint8_t sue_CLOCK_CONFIG, uint8_t sue_FLIGHT_PLAN_TYPE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, sue_TRAP_SOURCE); - _mav_put_int16_t(buf, 4, sue_RCON); - _mav_put_int16_t(buf, 6, sue_TRAP_FLAGS); - _mav_put_int16_t(buf, 8, sue_osc_fail_count); - _mav_put_uint8_t(buf, 10, sue_WIND_ESTIMATION); - _mav_put_uint8_t(buf, 11, sue_GPS_TYPE); - _mav_put_uint8_t(buf, 12, sue_DR); - _mav_put_uint8_t(buf, 13, sue_BOARD_TYPE); - _mav_put_uint8_t(buf, 14, sue_AIRFRAME); - _mav_put_uint8_t(buf, 15, sue_CLOCK_CONFIG); - _mav_put_uint8_t(buf, 16, sue_FLIGHT_PLAN_TYPE); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif -#else - mavlink_serial_udb_extra_f14_t *packet = (mavlink_serial_udb_extra_f14_t *)msgbuf; - packet->sue_TRAP_SOURCE = sue_TRAP_SOURCE; - packet->sue_RCON = sue_RCON; - packet->sue_TRAP_FLAGS = sue_TRAP_FLAGS; - packet->sue_osc_fail_count = sue_osc_fail_count; - packet->sue_WIND_ESTIMATION = sue_WIND_ESTIMATION; - packet->sue_GPS_TYPE = sue_GPS_TYPE; - packet->sue_DR = sue_DR; - packet->sue_BOARD_TYPE = sue_BOARD_TYPE; - packet->sue_AIRFRAME = sue_AIRFRAME; - packet->sue_CLOCK_CONFIG = sue_CLOCK_CONFIG; - packet->sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F14 UNPACKING - - -/** - * @brief Get field sue_WIND_ESTIMATION from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Wind Estimation Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_WIND_ESTIMATION(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field sue_GPS_TYPE from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Type of GPS Unit - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_GPS_TYPE(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field sue_DR from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Dead Reckoning Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_DR(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field sue_BOARD_TYPE from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Type of UDB Hardware - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_BOARD_TYPE(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 13); -} - -/** - * @brief Get field sue_AIRFRAME from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Type of Airframe - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_AIRFRAME(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field sue_RCON from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Reboot Regitster of DSPIC - */ -static inline int16_t mavlink_msg_serial_udb_extra_f14_get_sue_RCON(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 4); -} - -/** - * @brief Get field sue_TRAP_FLAGS from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Last dspic Trap Flags - */ -static inline int16_t mavlink_msg_serial_udb_extra_f14_get_sue_TRAP_FLAGS(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 6); -} - -/** - * @brief Get field sue_TRAP_SOURCE from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Type Program Address of Last Trap - */ -static inline uint32_t mavlink_msg_serial_udb_extra_f14_get_sue_TRAP_SOURCE(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field sue_osc_fail_count from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Number of Ocillator Failures - */ -static inline int16_t mavlink_msg_serial_udb_extra_f14_get_sue_osc_fail_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field sue_CLOCK_CONFIG from serial_udb_extra_f14 message - * - * @return Serial UDB Extra UDB Internal Clock Configuration - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_CLOCK_CONFIG(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 15); -} - -/** - * @brief Get field sue_FLIGHT_PLAN_TYPE from serial_udb_extra_f14 message - * - * @return Serial UDB Extra Type of Flight Plan - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_FLIGHT_PLAN_TYPE(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Decode a serial_udb_extra_f14 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f14 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f14_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f14_t* serial_udb_extra_f14) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f14->sue_TRAP_SOURCE = mavlink_msg_serial_udb_extra_f14_get_sue_TRAP_SOURCE(msg); - serial_udb_extra_f14->sue_RCON = mavlink_msg_serial_udb_extra_f14_get_sue_RCON(msg); - serial_udb_extra_f14->sue_TRAP_FLAGS = mavlink_msg_serial_udb_extra_f14_get_sue_TRAP_FLAGS(msg); - serial_udb_extra_f14->sue_osc_fail_count = mavlink_msg_serial_udb_extra_f14_get_sue_osc_fail_count(msg); - serial_udb_extra_f14->sue_WIND_ESTIMATION = mavlink_msg_serial_udb_extra_f14_get_sue_WIND_ESTIMATION(msg); - serial_udb_extra_f14->sue_GPS_TYPE = mavlink_msg_serial_udb_extra_f14_get_sue_GPS_TYPE(msg); - serial_udb_extra_f14->sue_DR = mavlink_msg_serial_udb_extra_f14_get_sue_DR(msg); - serial_udb_extra_f14->sue_BOARD_TYPE = mavlink_msg_serial_udb_extra_f14_get_sue_BOARD_TYPE(msg); - serial_udb_extra_f14->sue_AIRFRAME = mavlink_msg_serial_udb_extra_f14_get_sue_AIRFRAME(msg); - serial_udb_extra_f14->sue_CLOCK_CONFIG = mavlink_msg_serial_udb_extra_f14_get_sue_CLOCK_CONFIG(msg); - serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE = mavlink_msg_serial_udb_extra_f14_get_sue_FLIGHT_PLAN_TYPE(msg); -#else - memcpy(serial_udb_extra_f14, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h deleted file mode 100644 index 77666407d..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h +++ /dev/null @@ -1,234 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F15 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15 179 - -typedef struct __mavlink_serial_udb_extra_f15_t -{ - uint8_t sue_ID_VEHICLE_MODEL_NAME[40]; ///< Serial UDB Extra Model Name Of Vehicle - uint8_t sue_ID_VEHICLE_REGISTRATION[20]; ///< Serial UDB Extra Registraton Number of Vehicle -} mavlink_serial_udb_extra_f15_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN 60 -#define MAVLINK_MSG_ID_179_LEN 60 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC 7 -#define MAVLINK_MSG_ID_179_CRC 7 - -#define MAVLINK_MSG_SERIAL_UDB_EXTRA_F15_FIELD_SUE_ID_VEHICLE_MODEL_NAME_LEN 40 -#define MAVLINK_MSG_SERIAL_UDB_EXTRA_F15_FIELD_SUE_ID_VEHICLE_REGISTRATION_LEN 20 - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F15 { \ - "SERIAL_UDB_EXTRA_F15", \ - 2, \ - { { "sue_ID_VEHICLE_MODEL_NAME", NULL, MAVLINK_TYPE_UINT8_T, 40, 0, offsetof(mavlink_serial_udb_extra_f15_t, sue_ID_VEHICLE_MODEL_NAME) }, \ - { "sue_ID_VEHICLE_REGISTRATION", NULL, MAVLINK_TYPE_UINT8_T, 20, 40, offsetof(mavlink_serial_udb_extra_f15_t, sue_ID_VEHICLE_REGISTRATION) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f15 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_ID_VEHICLE_MODEL_NAME Serial UDB Extra Model Name Of Vehicle - * @param sue_ID_VEHICLE_REGISTRATION Serial UDB Extra Registraton Number of Vehicle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const uint8_t *sue_ID_VEHICLE_MODEL_NAME, const uint8_t *sue_ID_VEHICLE_REGISTRATION) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN]; - - _mav_put_uint8_t_array(buf, 0, sue_ID_VEHICLE_MODEL_NAME, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_VEHICLE_REGISTRATION, 20); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#else - mavlink_serial_udb_extra_f15_t packet; - - mav_array_memcpy(packet.sue_ID_VEHICLE_MODEL_NAME, sue_ID_VEHICLE_MODEL_NAME, sizeof(uint8_t)*40); - mav_array_memcpy(packet.sue_ID_VEHICLE_REGISTRATION, sue_ID_VEHICLE_REGISTRATION, sizeof(uint8_t)*20); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f15 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_ID_VEHICLE_MODEL_NAME Serial UDB Extra Model Name Of Vehicle - * @param sue_ID_VEHICLE_REGISTRATION Serial UDB Extra Registraton Number of Vehicle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const uint8_t *sue_ID_VEHICLE_MODEL_NAME,const uint8_t *sue_ID_VEHICLE_REGISTRATION) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN]; - - _mav_put_uint8_t_array(buf, 0, sue_ID_VEHICLE_MODEL_NAME, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_VEHICLE_REGISTRATION, 20); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#else - mavlink_serial_udb_extra_f15_t packet; - - mav_array_memcpy(packet.sue_ID_VEHICLE_MODEL_NAME, sue_ID_VEHICLE_MODEL_NAME, sizeof(uint8_t)*40); - mav_array_memcpy(packet.sue_ID_VEHICLE_REGISTRATION, sue_ID_VEHICLE_REGISTRATION, sizeof(uint8_t)*20); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f15 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f15 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f15_t* serial_udb_extra_f15) -{ - return mavlink_msg_serial_udb_extra_f15_pack(system_id, component_id, msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION); -} - -/** - * @brief Encode a serial_udb_extra_f15 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f15 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f15_t* serial_udb_extra_f15) -{ - return mavlink_msg_serial_udb_extra_f15_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION); -} - -/** - * @brief Send a serial_udb_extra_f15 message - * @param chan MAVLink channel to send the message - * - * @param sue_ID_VEHICLE_MODEL_NAME Serial UDB Extra Model Name Of Vehicle - * @param sue_ID_VEHICLE_REGISTRATION Serial UDB Extra Registraton Number of Vehicle - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f15_send(mavlink_channel_t chan, const uint8_t *sue_ID_VEHICLE_MODEL_NAME, const uint8_t *sue_ID_VEHICLE_REGISTRATION) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN]; - - _mav_put_uint8_t_array(buf, 0, sue_ID_VEHICLE_MODEL_NAME, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_VEHICLE_REGISTRATION, 20); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif -#else - mavlink_serial_udb_extra_f15_t packet; - - mav_array_memcpy(packet.sue_ID_VEHICLE_MODEL_NAME, sue_ID_VEHICLE_MODEL_NAME, sizeof(uint8_t)*40); - mav_array_memcpy(packet.sue_ID_VEHICLE_REGISTRATION, sue_ID_VEHICLE_REGISTRATION, sizeof(uint8_t)*20); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_serial_udb_extra_f15_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const uint8_t *sue_ID_VEHICLE_MODEL_NAME, const uint8_t *sue_ID_VEHICLE_REGISTRATION) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - - _mav_put_uint8_t_array(buf, 0, sue_ID_VEHICLE_MODEL_NAME, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_VEHICLE_REGISTRATION, 20); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif -#else - mavlink_serial_udb_extra_f15_t *packet = (mavlink_serial_udb_extra_f15_t *)msgbuf; - - mav_array_memcpy(packet->sue_ID_VEHICLE_MODEL_NAME, sue_ID_VEHICLE_MODEL_NAME, sizeof(uint8_t)*40); - mav_array_memcpy(packet->sue_ID_VEHICLE_REGISTRATION, sue_ID_VEHICLE_REGISTRATION, sizeof(uint8_t)*20); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F15 UNPACKING - - -/** - * @brief Get field sue_ID_VEHICLE_MODEL_NAME from serial_udb_extra_f15 message - * - * @return Serial UDB Extra Model Name Of Vehicle - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f15_get_sue_ID_VEHICLE_MODEL_NAME(const mavlink_message_t* msg, uint8_t *sue_ID_VEHICLE_MODEL_NAME) -{ - return _MAV_RETURN_uint8_t_array(msg, sue_ID_VEHICLE_MODEL_NAME, 40, 0); -} - -/** - * @brief Get field sue_ID_VEHICLE_REGISTRATION from serial_udb_extra_f15 message - * - * @return Serial UDB Extra Registraton Number of Vehicle - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f15_get_sue_ID_VEHICLE_REGISTRATION(const mavlink_message_t* msg, uint8_t *sue_ID_VEHICLE_REGISTRATION) -{ - return _MAV_RETURN_uint8_t_array(msg, sue_ID_VEHICLE_REGISTRATION, 20, 40); -} - -/** - * @brief Decode a serial_udb_extra_f15 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f15 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f15_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f15_t* serial_udb_extra_f15) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_serial_udb_extra_f15_get_sue_ID_VEHICLE_MODEL_NAME(msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME); - mavlink_msg_serial_udb_extra_f15_get_sue_ID_VEHICLE_REGISTRATION(msg, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION); -#else - memcpy(serial_udb_extra_f15, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h deleted file mode 100644 index ca9f0556f..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h +++ /dev/null @@ -1,234 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F16 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16 180 - -typedef struct __mavlink_serial_udb_extra_f16_t -{ - uint8_t sue_ID_LEAD_PILOT[40]; ///< Serial UDB Extra Name of Expected Lead Pilot - uint8_t sue_ID_DIY_DRONES_URL[70]; ///< Serial UDB Extra URL of Lead Pilot or Team -} mavlink_serial_udb_extra_f16_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN 110 -#define MAVLINK_MSG_ID_180_LEN 110 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC 222 -#define MAVLINK_MSG_ID_180_CRC 222 - -#define MAVLINK_MSG_SERIAL_UDB_EXTRA_F16_FIELD_SUE_ID_LEAD_PILOT_LEN 40 -#define MAVLINK_MSG_SERIAL_UDB_EXTRA_F16_FIELD_SUE_ID_DIY_DRONES_URL_LEN 70 - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F16 { \ - "SERIAL_UDB_EXTRA_F16", \ - 2, \ - { { "sue_ID_LEAD_PILOT", NULL, MAVLINK_TYPE_UINT8_T, 40, 0, offsetof(mavlink_serial_udb_extra_f16_t, sue_ID_LEAD_PILOT) }, \ - { "sue_ID_DIY_DRONES_URL", NULL, MAVLINK_TYPE_UINT8_T, 70, 40, offsetof(mavlink_serial_udb_extra_f16_t, sue_ID_DIY_DRONES_URL) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f16 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_ID_LEAD_PILOT Serial UDB Extra Name of Expected Lead Pilot - * @param sue_ID_DIY_DRONES_URL Serial UDB Extra URL of Lead Pilot or Team - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const uint8_t *sue_ID_LEAD_PILOT, const uint8_t *sue_ID_DIY_DRONES_URL) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN]; - - _mav_put_uint8_t_array(buf, 0, sue_ID_LEAD_PILOT, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_DIY_DRONES_URL, 70); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#else - mavlink_serial_udb_extra_f16_t packet; - - mav_array_memcpy(packet.sue_ID_LEAD_PILOT, sue_ID_LEAD_PILOT, sizeof(uint8_t)*40); - mav_array_memcpy(packet.sue_ID_DIY_DRONES_URL, sue_ID_DIY_DRONES_URL, sizeof(uint8_t)*70); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f16 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_ID_LEAD_PILOT Serial UDB Extra Name of Expected Lead Pilot - * @param sue_ID_DIY_DRONES_URL Serial UDB Extra URL of Lead Pilot or Team - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const uint8_t *sue_ID_LEAD_PILOT,const uint8_t *sue_ID_DIY_DRONES_URL) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN]; - - _mav_put_uint8_t_array(buf, 0, sue_ID_LEAD_PILOT, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_DIY_DRONES_URL, 70); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#else - mavlink_serial_udb_extra_f16_t packet; - - mav_array_memcpy(packet.sue_ID_LEAD_PILOT, sue_ID_LEAD_PILOT, sizeof(uint8_t)*40); - mav_array_memcpy(packet.sue_ID_DIY_DRONES_URL, sue_ID_DIY_DRONES_URL, sizeof(uint8_t)*70); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f16 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f16 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f16_t* serial_udb_extra_f16) -{ - return mavlink_msg_serial_udb_extra_f16_pack(system_id, component_id, msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL); -} - -/** - * @brief Encode a serial_udb_extra_f16 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f16 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f16_t* serial_udb_extra_f16) -{ - return mavlink_msg_serial_udb_extra_f16_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL); -} - -/** - * @brief Send a serial_udb_extra_f16 message - * @param chan MAVLink channel to send the message - * - * @param sue_ID_LEAD_PILOT Serial UDB Extra Name of Expected Lead Pilot - * @param sue_ID_DIY_DRONES_URL Serial UDB Extra URL of Lead Pilot or Team - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f16_send(mavlink_channel_t chan, const uint8_t *sue_ID_LEAD_PILOT, const uint8_t *sue_ID_DIY_DRONES_URL) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN]; - - _mav_put_uint8_t_array(buf, 0, sue_ID_LEAD_PILOT, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_DIY_DRONES_URL, 70); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif -#else - mavlink_serial_udb_extra_f16_t packet; - - mav_array_memcpy(packet.sue_ID_LEAD_PILOT, sue_ID_LEAD_PILOT, sizeof(uint8_t)*40); - mav_array_memcpy(packet.sue_ID_DIY_DRONES_URL, sue_ID_DIY_DRONES_URL, sizeof(uint8_t)*70); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_serial_udb_extra_f16_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const uint8_t *sue_ID_LEAD_PILOT, const uint8_t *sue_ID_DIY_DRONES_URL) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - - _mav_put_uint8_t_array(buf, 0, sue_ID_LEAD_PILOT, 40); - _mav_put_uint8_t_array(buf, 40, sue_ID_DIY_DRONES_URL, 70); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif -#else - mavlink_serial_udb_extra_f16_t *packet = (mavlink_serial_udb_extra_f16_t *)msgbuf; - - mav_array_memcpy(packet->sue_ID_LEAD_PILOT, sue_ID_LEAD_PILOT, sizeof(uint8_t)*40); - mav_array_memcpy(packet->sue_ID_DIY_DRONES_URL, sue_ID_DIY_DRONES_URL, sizeof(uint8_t)*70); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F16 UNPACKING - - -/** - * @brief Get field sue_ID_LEAD_PILOT from serial_udb_extra_f16 message - * - * @return Serial UDB Extra Name of Expected Lead Pilot - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f16_get_sue_ID_LEAD_PILOT(const mavlink_message_t* msg, uint8_t *sue_ID_LEAD_PILOT) -{ - return _MAV_RETURN_uint8_t_array(msg, sue_ID_LEAD_PILOT, 40, 0); -} - -/** - * @brief Get field sue_ID_DIY_DRONES_URL from serial_udb_extra_f16 message - * - * @return Serial UDB Extra URL of Lead Pilot or Team - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f16_get_sue_ID_DIY_DRONES_URL(const mavlink_message_t* msg, uint8_t *sue_ID_DIY_DRONES_URL) -{ - return _MAV_RETURN_uint8_t_array(msg, sue_ID_DIY_DRONES_URL, 70, 40); -} - -/** - * @brief Decode a serial_udb_extra_f16 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f16 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f16_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f16_t* serial_udb_extra_f16) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_serial_udb_extra_f16_get_sue_ID_LEAD_PILOT(msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT); - mavlink_msg_serial_udb_extra_f16_get_sue_ID_DIY_DRONES_URL(msg, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL); -#else - memcpy(serial_udb_extra_f16, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h deleted file mode 100644 index 41b9f09c0..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h +++ /dev/null @@ -1,857 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F2_A PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A 170 - -typedef struct __mavlink_serial_udb_extra_f2_a_t -{ - uint32_t sue_time; ///< Serial UDB Extra Time - int32_t sue_latitude; ///< Serial UDB Extra Latitude - int32_t sue_longitude; ///< Serial UDB Extra Longitude - int32_t sue_altitude; ///< Serial UDB Extra Altitude - uint16_t sue_waypoint_index; ///< Serial UDB Extra Waypoint Index - int16_t sue_rmat0; ///< Serial UDB Extra Rmat 0 - int16_t sue_rmat1; ///< Serial UDB Extra Rmat 1 - int16_t sue_rmat2; ///< Serial UDB Extra Rmat 2 - int16_t sue_rmat3; ///< Serial UDB Extra Rmat 3 - int16_t sue_rmat4; ///< Serial UDB Extra Rmat 4 - int16_t sue_rmat5; ///< Serial UDB Extra Rmat 5 - int16_t sue_rmat6; ///< Serial UDB Extra Rmat 6 - int16_t sue_rmat7; ///< Serial UDB Extra Rmat 7 - int16_t sue_rmat8; ///< Serial UDB Extra Rmat 8 - uint16_t sue_cog; ///< Serial UDB Extra GPS Course Over Ground - int16_t sue_sog; ///< Serial UDB Extra Speed Over Ground - uint16_t sue_cpu_load; ///< Serial UDB Extra CPU Load - int16_t sue_voltage_milis; ///< Serial UDB Extra Voltage in MilliVolts - uint16_t sue_air_speed_3DIMU; ///< Serial UDB Extra 3D IMU Air Speed - int16_t sue_estimated_wind_0; ///< Serial UDB Extra Estimated Wind 0 - int16_t sue_estimated_wind_1; ///< Serial UDB Extra Estimated Wind 1 - int16_t sue_estimated_wind_2; ///< Serial UDB Extra Estimated Wind 2 - int16_t sue_magFieldEarth0; ///< Serial UDB Extra Magnetic Field Earth 0 - int16_t sue_magFieldEarth1; ///< Serial UDB Extra Magnetic Field Earth 1 - int16_t sue_magFieldEarth2; ///< Serial UDB Extra Magnetic Field Earth 2 - int16_t sue_svs; ///< Serial UDB Extra Number of Sattelites in View - int16_t sue_hdop; ///< Serial UDB Extra GPS Horizontal Dilution of Precision - uint8_t sue_status; ///< Serial UDB Extra Status -} mavlink_serial_udb_extra_f2_a_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN 63 -#define MAVLINK_MSG_ID_170_LEN 63 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC 150 -#define MAVLINK_MSG_ID_170_CRC 150 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F2_A { \ - "SERIAL_UDB_EXTRA_F2_A", \ - 28, \ - { { "sue_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_time) }, \ - { "sue_latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_latitude) }, \ - { "sue_longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_longitude) }, \ - { "sue_altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_altitude) }, \ - { "sue_waypoint_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_waypoint_index) }, \ - { "sue_rmat0", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat0) }, \ - { "sue_rmat1", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat1) }, \ - { "sue_rmat2", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat2) }, \ - { "sue_rmat3", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat3) }, \ - { "sue_rmat4", NULL, MAVLINK_TYPE_INT16_T, 0, 26, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat4) }, \ - { "sue_rmat5", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat5) }, \ - { "sue_rmat6", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat6) }, \ - { "sue_rmat7", NULL, MAVLINK_TYPE_INT16_T, 0, 32, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat7) }, \ - { "sue_rmat8", NULL, MAVLINK_TYPE_INT16_T, 0, 34, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat8) }, \ - { "sue_cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_cog) }, \ - { "sue_sog", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_sog) }, \ - { "sue_cpu_load", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_cpu_load) }, \ - { "sue_voltage_milis", NULL, MAVLINK_TYPE_INT16_T, 0, 42, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_voltage_milis) }, \ - { "sue_air_speed_3DIMU", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_air_speed_3DIMU) }, \ - { "sue_estimated_wind_0", NULL, MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_estimated_wind_0) }, \ - { "sue_estimated_wind_1", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_estimated_wind_1) }, \ - { "sue_estimated_wind_2", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_estimated_wind_2) }, \ - { "sue_magFieldEarth0", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_magFieldEarth0) }, \ - { "sue_magFieldEarth1", NULL, MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_magFieldEarth1) }, \ - { "sue_magFieldEarth2", NULL, MAVLINK_TYPE_INT16_T, 0, 56, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_magFieldEarth2) }, \ - { "sue_svs", NULL, MAVLINK_TYPE_INT16_T, 0, 58, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_svs) }, \ - { "sue_hdop", NULL, MAVLINK_TYPE_INT16_T, 0, 60, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_hdop) }, \ - { "sue_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 62, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_status) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f2_a message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_time Serial UDB Extra Time - * @param sue_status Serial UDB Extra Status - * @param sue_latitude Serial UDB Extra Latitude - * @param sue_longitude Serial UDB Extra Longitude - * @param sue_altitude Serial UDB Extra Altitude - * @param sue_waypoint_index Serial UDB Extra Waypoint Index - * @param sue_rmat0 Serial UDB Extra Rmat 0 - * @param sue_rmat1 Serial UDB Extra Rmat 1 - * @param sue_rmat2 Serial UDB Extra Rmat 2 - * @param sue_rmat3 Serial UDB Extra Rmat 3 - * @param sue_rmat4 Serial UDB Extra Rmat 4 - * @param sue_rmat5 Serial UDB Extra Rmat 5 - * @param sue_rmat6 Serial UDB Extra Rmat 6 - * @param sue_rmat7 Serial UDB Extra Rmat 7 - * @param sue_rmat8 Serial UDB Extra Rmat 8 - * @param sue_cog Serial UDB Extra GPS Course Over Ground - * @param sue_sog Serial UDB Extra Speed Over Ground - * @param sue_cpu_load Serial UDB Extra CPU Load - * @param sue_voltage_milis Serial UDB Extra Voltage in MilliVolts - * @param sue_air_speed_3DIMU Serial UDB Extra 3D IMU Air Speed - * @param sue_estimated_wind_0 Serial UDB Extra Estimated Wind 0 - * @param sue_estimated_wind_1 Serial UDB Extra Estimated Wind 1 - * @param sue_estimated_wind_2 Serial UDB Extra Estimated Wind 2 - * @param sue_magFieldEarth0 Serial UDB Extra Magnetic Field Earth 0 - * @param sue_magFieldEarth1 Serial UDB Extra Magnetic Field Earth 1 - * @param sue_magFieldEarth2 Serial UDB Extra Magnetic Field Earth 2 - * @param sue_svs Serial UDB Extra Number of Sattelites in View - * @param sue_hdop Serial UDB Extra GPS Horizontal Dilution of Precision - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t sue_time, uint8_t sue_status, int32_t sue_latitude, int32_t sue_longitude, int32_t sue_altitude, uint16_t sue_waypoint_index, int16_t sue_rmat0, int16_t sue_rmat1, int16_t sue_rmat2, int16_t sue_rmat3, int16_t sue_rmat4, int16_t sue_rmat5, int16_t sue_rmat6, int16_t sue_rmat7, int16_t sue_rmat8, uint16_t sue_cog, int16_t sue_sog, uint16_t sue_cpu_load, int16_t sue_voltage_milis, uint16_t sue_air_speed_3DIMU, int16_t sue_estimated_wind_0, int16_t sue_estimated_wind_1, int16_t sue_estimated_wind_2, int16_t sue_magFieldEarth0, int16_t sue_magFieldEarth1, int16_t sue_magFieldEarth2, int16_t sue_svs, int16_t sue_hdop) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN]; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_int32_t(buf, 4, sue_latitude); - _mav_put_int32_t(buf, 8, sue_longitude); - _mav_put_int32_t(buf, 12, sue_altitude); - _mav_put_uint16_t(buf, 16, sue_waypoint_index); - _mav_put_int16_t(buf, 18, sue_rmat0); - _mav_put_int16_t(buf, 20, sue_rmat1); - _mav_put_int16_t(buf, 22, sue_rmat2); - _mav_put_int16_t(buf, 24, sue_rmat3); - _mav_put_int16_t(buf, 26, sue_rmat4); - _mav_put_int16_t(buf, 28, sue_rmat5); - _mav_put_int16_t(buf, 30, sue_rmat6); - _mav_put_int16_t(buf, 32, sue_rmat7); - _mav_put_int16_t(buf, 34, sue_rmat8); - _mav_put_uint16_t(buf, 36, sue_cog); - _mav_put_int16_t(buf, 38, sue_sog); - _mav_put_uint16_t(buf, 40, sue_cpu_load); - _mav_put_int16_t(buf, 42, sue_voltage_milis); - _mav_put_uint16_t(buf, 44, sue_air_speed_3DIMU); - _mav_put_int16_t(buf, 46, sue_estimated_wind_0); - _mav_put_int16_t(buf, 48, sue_estimated_wind_1); - _mav_put_int16_t(buf, 50, sue_estimated_wind_2); - _mav_put_int16_t(buf, 52, sue_magFieldEarth0); - _mav_put_int16_t(buf, 54, sue_magFieldEarth1); - _mav_put_int16_t(buf, 56, sue_magFieldEarth2); - _mav_put_int16_t(buf, 58, sue_svs); - _mav_put_int16_t(buf, 60, sue_hdop); - _mav_put_uint8_t(buf, 62, sue_status); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#else - mavlink_serial_udb_extra_f2_a_t packet; - packet.sue_time = sue_time; - packet.sue_latitude = sue_latitude; - packet.sue_longitude = sue_longitude; - packet.sue_altitude = sue_altitude; - packet.sue_waypoint_index = sue_waypoint_index; - packet.sue_rmat0 = sue_rmat0; - packet.sue_rmat1 = sue_rmat1; - packet.sue_rmat2 = sue_rmat2; - packet.sue_rmat3 = sue_rmat3; - packet.sue_rmat4 = sue_rmat4; - packet.sue_rmat5 = sue_rmat5; - packet.sue_rmat6 = sue_rmat6; - packet.sue_rmat7 = sue_rmat7; - packet.sue_rmat8 = sue_rmat8; - packet.sue_cog = sue_cog; - packet.sue_sog = sue_sog; - packet.sue_cpu_load = sue_cpu_load; - packet.sue_voltage_milis = sue_voltage_milis; - packet.sue_air_speed_3DIMU = sue_air_speed_3DIMU; - packet.sue_estimated_wind_0 = sue_estimated_wind_0; - packet.sue_estimated_wind_1 = sue_estimated_wind_1; - packet.sue_estimated_wind_2 = sue_estimated_wind_2; - packet.sue_magFieldEarth0 = sue_magFieldEarth0; - packet.sue_magFieldEarth1 = sue_magFieldEarth1; - packet.sue_magFieldEarth2 = sue_magFieldEarth2; - packet.sue_svs = sue_svs; - packet.sue_hdop = sue_hdop; - packet.sue_status = sue_status; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f2_a message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_time Serial UDB Extra Time - * @param sue_status Serial UDB Extra Status - * @param sue_latitude Serial UDB Extra Latitude - * @param sue_longitude Serial UDB Extra Longitude - * @param sue_altitude Serial UDB Extra Altitude - * @param sue_waypoint_index Serial UDB Extra Waypoint Index - * @param sue_rmat0 Serial UDB Extra Rmat 0 - * @param sue_rmat1 Serial UDB Extra Rmat 1 - * @param sue_rmat2 Serial UDB Extra Rmat 2 - * @param sue_rmat3 Serial UDB Extra Rmat 3 - * @param sue_rmat4 Serial UDB Extra Rmat 4 - * @param sue_rmat5 Serial UDB Extra Rmat 5 - * @param sue_rmat6 Serial UDB Extra Rmat 6 - * @param sue_rmat7 Serial UDB Extra Rmat 7 - * @param sue_rmat8 Serial UDB Extra Rmat 8 - * @param sue_cog Serial UDB Extra GPS Course Over Ground - * @param sue_sog Serial UDB Extra Speed Over Ground - * @param sue_cpu_load Serial UDB Extra CPU Load - * @param sue_voltage_milis Serial UDB Extra Voltage in MilliVolts - * @param sue_air_speed_3DIMU Serial UDB Extra 3D IMU Air Speed - * @param sue_estimated_wind_0 Serial UDB Extra Estimated Wind 0 - * @param sue_estimated_wind_1 Serial UDB Extra Estimated Wind 1 - * @param sue_estimated_wind_2 Serial UDB Extra Estimated Wind 2 - * @param sue_magFieldEarth0 Serial UDB Extra Magnetic Field Earth 0 - * @param sue_magFieldEarth1 Serial UDB Extra Magnetic Field Earth 1 - * @param sue_magFieldEarth2 Serial UDB Extra Magnetic Field Earth 2 - * @param sue_svs Serial UDB Extra Number of Sattelites in View - * @param sue_hdop Serial UDB Extra GPS Horizontal Dilution of Precision - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t sue_time,uint8_t sue_status,int32_t sue_latitude,int32_t sue_longitude,int32_t sue_altitude,uint16_t sue_waypoint_index,int16_t sue_rmat0,int16_t sue_rmat1,int16_t sue_rmat2,int16_t sue_rmat3,int16_t sue_rmat4,int16_t sue_rmat5,int16_t sue_rmat6,int16_t sue_rmat7,int16_t sue_rmat8,uint16_t sue_cog,int16_t sue_sog,uint16_t sue_cpu_load,int16_t sue_voltage_milis,uint16_t sue_air_speed_3DIMU,int16_t sue_estimated_wind_0,int16_t sue_estimated_wind_1,int16_t sue_estimated_wind_2,int16_t sue_magFieldEarth0,int16_t sue_magFieldEarth1,int16_t sue_magFieldEarth2,int16_t sue_svs,int16_t sue_hdop) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN]; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_int32_t(buf, 4, sue_latitude); - _mav_put_int32_t(buf, 8, sue_longitude); - _mav_put_int32_t(buf, 12, sue_altitude); - _mav_put_uint16_t(buf, 16, sue_waypoint_index); - _mav_put_int16_t(buf, 18, sue_rmat0); - _mav_put_int16_t(buf, 20, sue_rmat1); - _mav_put_int16_t(buf, 22, sue_rmat2); - _mav_put_int16_t(buf, 24, sue_rmat3); - _mav_put_int16_t(buf, 26, sue_rmat4); - _mav_put_int16_t(buf, 28, sue_rmat5); - _mav_put_int16_t(buf, 30, sue_rmat6); - _mav_put_int16_t(buf, 32, sue_rmat7); - _mav_put_int16_t(buf, 34, sue_rmat8); - _mav_put_uint16_t(buf, 36, sue_cog); - _mav_put_int16_t(buf, 38, sue_sog); - _mav_put_uint16_t(buf, 40, sue_cpu_load); - _mav_put_int16_t(buf, 42, sue_voltage_milis); - _mav_put_uint16_t(buf, 44, sue_air_speed_3DIMU); - _mav_put_int16_t(buf, 46, sue_estimated_wind_0); - _mav_put_int16_t(buf, 48, sue_estimated_wind_1); - _mav_put_int16_t(buf, 50, sue_estimated_wind_2); - _mav_put_int16_t(buf, 52, sue_magFieldEarth0); - _mav_put_int16_t(buf, 54, sue_magFieldEarth1); - _mav_put_int16_t(buf, 56, sue_magFieldEarth2); - _mav_put_int16_t(buf, 58, sue_svs); - _mav_put_int16_t(buf, 60, sue_hdop); - _mav_put_uint8_t(buf, 62, sue_status); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#else - mavlink_serial_udb_extra_f2_a_t packet; - packet.sue_time = sue_time; - packet.sue_latitude = sue_latitude; - packet.sue_longitude = sue_longitude; - packet.sue_altitude = sue_altitude; - packet.sue_waypoint_index = sue_waypoint_index; - packet.sue_rmat0 = sue_rmat0; - packet.sue_rmat1 = sue_rmat1; - packet.sue_rmat2 = sue_rmat2; - packet.sue_rmat3 = sue_rmat3; - packet.sue_rmat4 = sue_rmat4; - packet.sue_rmat5 = sue_rmat5; - packet.sue_rmat6 = sue_rmat6; - packet.sue_rmat7 = sue_rmat7; - packet.sue_rmat8 = sue_rmat8; - packet.sue_cog = sue_cog; - packet.sue_sog = sue_sog; - packet.sue_cpu_load = sue_cpu_load; - packet.sue_voltage_milis = sue_voltage_milis; - packet.sue_air_speed_3DIMU = sue_air_speed_3DIMU; - packet.sue_estimated_wind_0 = sue_estimated_wind_0; - packet.sue_estimated_wind_1 = sue_estimated_wind_1; - packet.sue_estimated_wind_2 = sue_estimated_wind_2; - packet.sue_magFieldEarth0 = sue_magFieldEarth0; - packet.sue_magFieldEarth1 = sue_magFieldEarth1; - packet.sue_magFieldEarth2 = sue_magFieldEarth2; - packet.sue_svs = sue_svs; - packet.sue_hdop = sue_hdop; - packet.sue_status = sue_status; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f2_a struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f2_a C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_a_t* serial_udb_extra_f2_a) -{ - return mavlink_msg_serial_udb_extra_f2_a_pack(system_id, component_id, msg, serial_udb_extra_f2_a->sue_time, serial_udb_extra_f2_a->sue_status, serial_udb_extra_f2_a->sue_latitude, serial_udb_extra_f2_a->sue_longitude, serial_udb_extra_f2_a->sue_altitude, serial_udb_extra_f2_a->sue_waypoint_index, serial_udb_extra_f2_a->sue_rmat0, serial_udb_extra_f2_a->sue_rmat1, serial_udb_extra_f2_a->sue_rmat2, serial_udb_extra_f2_a->sue_rmat3, serial_udb_extra_f2_a->sue_rmat4, serial_udb_extra_f2_a->sue_rmat5, serial_udb_extra_f2_a->sue_rmat6, serial_udb_extra_f2_a->sue_rmat7, serial_udb_extra_f2_a->sue_rmat8, serial_udb_extra_f2_a->sue_cog, serial_udb_extra_f2_a->sue_sog, serial_udb_extra_f2_a->sue_cpu_load, serial_udb_extra_f2_a->sue_voltage_milis, serial_udb_extra_f2_a->sue_air_speed_3DIMU, serial_udb_extra_f2_a->sue_estimated_wind_0, serial_udb_extra_f2_a->sue_estimated_wind_1, serial_udb_extra_f2_a->sue_estimated_wind_2, serial_udb_extra_f2_a->sue_magFieldEarth0, serial_udb_extra_f2_a->sue_magFieldEarth1, serial_udb_extra_f2_a->sue_magFieldEarth2, serial_udb_extra_f2_a->sue_svs, serial_udb_extra_f2_a->sue_hdop); -} - -/** - * @brief Encode a serial_udb_extra_f2_a struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f2_a C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_a_t* serial_udb_extra_f2_a) -{ - return mavlink_msg_serial_udb_extra_f2_a_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f2_a->sue_time, serial_udb_extra_f2_a->sue_status, serial_udb_extra_f2_a->sue_latitude, serial_udb_extra_f2_a->sue_longitude, serial_udb_extra_f2_a->sue_altitude, serial_udb_extra_f2_a->sue_waypoint_index, serial_udb_extra_f2_a->sue_rmat0, serial_udb_extra_f2_a->sue_rmat1, serial_udb_extra_f2_a->sue_rmat2, serial_udb_extra_f2_a->sue_rmat3, serial_udb_extra_f2_a->sue_rmat4, serial_udb_extra_f2_a->sue_rmat5, serial_udb_extra_f2_a->sue_rmat6, serial_udb_extra_f2_a->sue_rmat7, serial_udb_extra_f2_a->sue_rmat8, serial_udb_extra_f2_a->sue_cog, serial_udb_extra_f2_a->sue_sog, serial_udb_extra_f2_a->sue_cpu_load, serial_udb_extra_f2_a->sue_voltage_milis, serial_udb_extra_f2_a->sue_air_speed_3DIMU, serial_udb_extra_f2_a->sue_estimated_wind_0, serial_udb_extra_f2_a->sue_estimated_wind_1, serial_udb_extra_f2_a->sue_estimated_wind_2, serial_udb_extra_f2_a->sue_magFieldEarth0, serial_udb_extra_f2_a->sue_magFieldEarth1, serial_udb_extra_f2_a->sue_magFieldEarth2, serial_udb_extra_f2_a->sue_svs, serial_udb_extra_f2_a->sue_hdop); -} - -/** - * @brief Send a serial_udb_extra_f2_a message - * @param chan MAVLink channel to send the message - * - * @param sue_time Serial UDB Extra Time - * @param sue_status Serial UDB Extra Status - * @param sue_latitude Serial UDB Extra Latitude - * @param sue_longitude Serial UDB Extra Longitude - * @param sue_altitude Serial UDB Extra Altitude - * @param sue_waypoint_index Serial UDB Extra Waypoint Index - * @param sue_rmat0 Serial UDB Extra Rmat 0 - * @param sue_rmat1 Serial UDB Extra Rmat 1 - * @param sue_rmat2 Serial UDB Extra Rmat 2 - * @param sue_rmat3 Serial UDB Extra Rmat 3 - * @param sue_rmat4 Serial UDB Extra Rmat 4 - * @param sue_rmat5 Serial UDB Extra Rmat 5 - * @param sue_rmat6 Serial UDB Extra Rmat 6 - * @param sue_rmat7 Serial UDB Extra Rmat 7 - * @param sue_rmat8 Serial UDB Extra Rmat 8 - * @param sue_cog Serial UDB Extra GPS Course Over Ground - * @param sue_sog Serial UDB Extra Speed Over Ground - * @param sue_cpu_load Serial UDB Extra CPU Load - * @param sue_voltage_milis Serial UDB Extra Voltage in MilliVolts - * @param sue_air_speed_3DIMU Serial UDB Extra 3D IMU Air Speed - * @param sue_estimated_wind_0 Serial UDB Extra Estimated Wind 0 - * @param sue_estimated_wind_1 Serial UDB Extra Estimated Wind 1 - * @param sue_estimated_wind_2 Serial UDB Extra Estimated Wind 2 - * @param sue_magFieldEarth0 Serial UDB Extra Magnetic Field Earth 0 - * @param sue_magFieldEarth1 Serial UDB Extra Magnetic Field Earth 1 - * @param sue_magFieldEarth2 Serial UDB Extra Magnetic Field Earth 2 - * @param sue_svs Serial UDB Extra Number of Sattelites in View - * @param sue_hdop Serial UDB Extra GPS Horizontal Dilution of Precision - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f2_a_send(mavlink_channel_t chan, uint32_t sue_time, uint8_t sue_status, int32_t sue_latitude, int32_t sue_longitude, int32_t sue_altitude, uint16_t sue_waypoint_index, int16_t sue_rmat0, int16_t sue_rmat1, int16_t sue_rmat2, int16_t sue_rmat3, int16_t sue_rmat4, int16_t sue_rmat5, int16_t sue_rmat6, int16_t sue_rmat7, int16_t sue_rmat8, uint16_t sue_cog, int16_t sue_sog, uint16_t sue_cpu_load, int16_t sue_voltage_milis, uint16_t sue_air_speed_3DIMU, int16_t sue_estimated_wind_0, int16_t sue_estimated_wind_1, int16_t sue_estimated_wind_2, int16_t sue_magFieldEarth0, int16_t sue_magFieldEarth1, int16_t sue_magFieldEarth2, int16_t sue_svs, int16_t sue_hdop) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN]; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_int32_t(buf, 4, sue_latitude); - _mav_put_int32_t(buf, 8, sue_longitude); - _mav_put_int32_t(buf, 12, sue_altitude); - _mav_put_uint16_t(buf, 16, sue_waypoint_index); - _mav_put_int16_t(buf, 18, sue_rmat0); - _mav_put_int16_t(buf, 20, sue_rmat1); - _mav_put_int16_t(buf, 22, sue_rmat2); - _mav_put_int16_t(buf, 24, sue_rmat3); - _mav_put_int16_t(buf, 26, sue_rmat4); - _mav_put_int16_t(buf, 28, sue_rmat5); - _mav_put_int16_t(buf, 30, sue_rmat6); - _mav_put_int16_t(buf, 32, sue_rmat7); - _mav_put_int16_t(buf, 34, sue_rmat8); - _mav_put_uint16_t(buf, 36, sue_cog); - _mav_put_int16_t(buf, 38, sue_sog); - _mav_put_uint16_t(buf, 40, sue_cpu_load); - _mav_put_int16_t(buf, 42, sue_voltage_milis); - _mav_put_uint16_t(buf, 44, sue_air_speed_3DIMU); - _mav_put_int16_t(buf, 46, sue_estimated_wind_0); - _mav_put_int16_t(buf, 48, sue_estimated_wind_1); - _mav_put_int16_t(buf, 50, sue_estimated_wind_2); - _mav_put_int16_t(buf, 52, sue_magFieldEarth0); - _mav_put_int16_t(buf, 54, sue_magFieldEarth1); - _mav_put_int16_t(buf, 56, sue_magFieldEarth2); - _mav_put_int16_t(buf, 58, sue_svs); - _mav_put_int16_t(buf, 60, sue_hdop); - _mav_put_uint8_t(buf, 62, sue_status); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif -#else - mavlink_serial_udb_extra_f2_a_t packet; - packet.sue_time = sue_time; - packet.sue_latitude = sue_latitude; - packet.sue_longitude = sue_longitude; - packet.sue_altitude = sue_altitude; - packet.sue_waypoint_index = sue_waypoint_index; - packet.sue_rmat0 = sue_rmat0; - packet.sue_rmat1 = sue_rmat1; - packet.sue_rmat2 = sue_rmat2; - packet.sue_rmat3 = sue_rmat3; - packet.sue_rmat4 = sue_rmat4; - packet.sue_rmat5 = sue_rmat5; - packet.sue_rmat6 = sue_rmat6; - packet.sue_rmat7 = sue_rmat7; - packet.sue_rmat8 = sue_rmat8; - packet.sue_cog = sue_cog; - packet.sue_sog = sue_sog; - packet.sue_cpu_load = sue_cpu_load; - packet.sue_voltage_milis = sue_voltage_milis; - packet.sue_air_speed_3DIMU = sue_air_speed_3DIMU; - packet.sue_estimated_wind_0 = sue_estimated_wind_0; - packet.sue_estimated_wind_1 = sue_estimated_wind_1; - packet.sue_estimated_wind_2 = sue_estimated_wind_2; - packet.sue_magFieldEarth0 = sue_magFieldEarth0; - packet.sue_magFieldEarth1 = sue_magFieldEarth1; - packet.sue_magFieldEarth2 = sue_magFieldEarth2; - packet.sue_svs = sue_svs; - packet.sue_hdop = sue_hdop; - packet.sue_status = sue_status; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_serial_udb_extra_f2_a_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t sue_time, uint8_t sue_status, int32_t sue_latitude, int32_t sue_longitude, int32_t sue_altitude, uint16_t sue_waypoint_index, int16_t sue_rmat0, int16_t sue_rmat1, int16_t sue_rmat2, int16_t sue_rmat3, int16_t sue_rmat4, int16_t sue_rmat5, int16_t sue_rmat6, int16_t sue_rmat7, int16_t sue_rmat8, uint16_t sue_cog, int16_t sue_sog, uint16_t sue_cpu_load, int16_t sue_voltage_milis, uint16_t sue_air_speed_3DIMU, int16_t sue_estimated_wind_0, int16_t sue_estimated_wind_1, int16_t sue_estimated_wind_2, int16_t sue_magFieldEarth0, int16_t sue_magFieldEarth1, int16_t sue_magFieldEarth2, int16_t sue_svs, int16_t sue_hdop) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_int32_t(buf, 4, sue_latitude); - _mav_put_int32_t(buf, 8, sue_longitude); - _mav_put_int32_t(buf, 12, sue_altitude); - _mav_put_uint16_t(buf, 16, sue_waypoint_index); - _mav_put_int16_t(buf, 18, sue_rmat0); - _mav_put_int16_t(buf, 20, sue_rmat1); - _mav_put_int16_t(buf, 22, sue_rmat2); - _mav_put_int16_t(buf, 24, sue_rmat3); - _mav_put_int16_t(buf, 26, sue_rmat4); - _mav_put_int16_t(buf, 28, sue_rmat5); - _mav_put_int16_t(buf, 30, sue_rmat6); - _mav_put_int16_t(buf, 32, sue_rmat7); - _mav_put_int16_t(buf, 34, sue_rmat8); - _mav_put_uint16_t(buf, 36, sue_cog); - _mav_put_int16_t(buf, 38, sue_sog); - _mav_put_uint16_t(buf, 40, sue_cpu_load); - _mav_put_int16_t(buf, 42, sue_voltage_milis); - _mav_put_uint16_t(buf, 44, sue_air_speed_3DIMU); - _mav_put_int16_t(buf, 46, sue_estimated_wind_0); - _mav_put_int16_t(buf, 48, sue_estimated_wind_1); - _mav_put_int16_t(buf, 50, sue_estimated_wind_2); - _mav_put_int16_t(buf, 52, sue_magFieldEarth0); - _mav_put_int16_t(buf, 54, sue_magFieldEarth1); - _mav_put_int16_t(buf, 56, sue_magFieldEarth2); - _mav_put_int16_t(buf, 58, sue_svs); - _mav_put_int16_t(buf, 60, sue_hdop); - _mav_put_uint8_t(buf, 62, sue_status); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif -#else - mavlink_serial_udb_extra_f2_a_t *packet = (mavlink_serial_udb_extra_f2_a_t *)msgbuf; - packet->sue_time = sue_time; - packet->sue_latitude = sue_latitude; - packet->sue_longitude = sue_longitude; - packet->sue_altitude = sue_altitude; - packet->sue_waypoint_index = sue_waypoint_index; - packet->sue_rmat0 = sue_rmat0; - packet->sue_rmat1 = sue_rmat1; - packet->sue_rmat2 = sue_rmat2; - packet->sue_rmat3 = sue_rmat3; - packet->sue_rmat4 = sue_rmat4; - packet->sue_rmat5 = sue_rmat5; - packet->sue_rmat6 = sue_rmat6; - packet->sue_rmat7 = sue_rmat7; - packet->sue_rmat8 = sue_rmat8; - packet->sue_cog = sue_cog; - packet->sue_sog = sue_sog; - packet->sue_cpu_load = sue_cpu_load; - packet->sue_voltage_milis = sue_voltage_milis; - packet->sue_air_speed_3DIMU = sue_air_speed_3DIMU; - packet->sue_estimated_wind_0 = sue_estimated_wind_0; - packet->sue_estimated_wind_1 = sue_estimated_wind_1; - packet->sue_estimated_wind_2 = sue_estimated_wind_2; - packet->sue_magFieldEarth0 = sue_magFieldEarth0; - packet->sue_magFieldEarth1 = sue_magFieldEarth1; - packet->sue_magFieldEarth2 = sue_magFieldEarth2; - packet->sue_svs = sue_svs; - packet->sue_hdop = sue_hdop; - packet->sue_status = sue_status; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F2_A UNPACKING - - -/** - * @brief Get field sue_time from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Time - */ -static inline uint32_t mavlink_msg_serial_udb_extra_f2_a_get_sue_time(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field sue_status from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Status - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f2_a_get_sue_status(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 62); -} - -/** - * @brief Get field sue_latitude from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Latitude - */ -static inline int32_t mavlink_msg_serial_udb_extra_f2_a_get_sue_latitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field sue_longitude from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Longitude - */ -static inline int32_t mavlink_msg_serial_udb_extra_f2_a_get_sue_longitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field sue_altitude from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Altitude - */ -static inline int32_t mavlink_msg_serial_udb_extra_f2_a_get_sue_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 12); -} - -/** - * @brief Get field sue_waypoint_index from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Waypoint Index - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_waypoint_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field sue_rmat0 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 0 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat0(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 18); -} - -/** - * @brief Get field sue_rmat1 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 1 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Get field sue_rmat2 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 2 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 22); -} - -/** - * @brief Get field sue_rmat3 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 3 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 24); -} - -/** - * @brief Get field sue_rmat4 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 4 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 26); -} - -/** - * @brief Get field sue_rmat5 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 5 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 28); -} - -/** - * @brief Get field sue_rmat6 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 6 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 30); -} - -/** - * @brief Get field sue_rmat7 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 7 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 32); -} - -/** - * @brief Get field sue_rmat8 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Rmat 8 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 34); -} - -/** - * @brief Get field sue_cog from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra GPS Course Over Ground - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_cog(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 36); -} - -/** - * @brief Get field sue_sog from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Speed Over Ground - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_sog(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 38); -} - -/** - * @brief Get field sue_cpu_load from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra CPU Load - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_cpu_load(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 40); -} - -/** - * @brief Get field sue_voltage_milis from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Voltage in MilliVolts - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_voltage_milis(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 42); -} - -/** - * @brief Get field sue_air_speed_3DIMU from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra 3D IMU Air Speed - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_air_speed_3DIMU(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 44); -} - -/** - * @brief Get field sue_estimated_wind_0 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Estimated Wind 0 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_0(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 46); -} - -/** - * @brief Get field sue_estimated_wind_1 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Estimated Wind 1 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 48); -} - -/** - * @brief Get field sue_estimated_wind_2 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Estimated Wind 2 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 50); -} - -/** - * @brief Get field sue_magFieldEarth0 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Magnetic Field Earth 0 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth0(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 52); -} - -/** - * @brief Get field sue_magFieldEarth1 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Magnetic Field Earth 1 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 54); -} - -/** - * @brief Get field sue_magFieldEarth2 from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Magnetic Field Earth 2 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 56); -} - -/** - * @brief Get field sue_svs from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra Number of Sattelites in View - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_svs(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 58); -} - -/** - * @brief Get field sue_hdop from serial_udb_extra_f2_a message - * - * @return Serial UDB Extra GPS Horizontal Dilution of Precision - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_hdop(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 60); -} - -/** - * @brief Decode a serial_udb_extra_f2_a message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f2_a C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f2_a_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f2_a_t* serial_udb_extra_f2_a) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f2_a->sue_time = mavlink_msg_serial_udb_extra_f2_a_get_sue_time(msg); - serial_udb_extra_f2_a->sue_latitude = mavlink_msg_serial_udb_extra_f2_a_get_sue_latitude(msg); - serial_udb_extra_f2_a->sue_longitude = mavlink_msg_serial_udb_extra_f2_a_get_sue_longitude(msg); - serial_udb_extra_f2_a->sue_altitude = mavlink_msg_serial_udb_extra_f2_a_get_sue_altitude(msg); - serial_udb_extra_f2_a->sue_waypoint_index = mavlink_msg_serial_udb_extra_f2_a_get_sue_waypoint_index(msg); - serial_udb_extra_f2_a->sue_rmat0 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat0(msg); - serial_udb_extra_f2_a->sue_rmat1 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat1(msg); - serial_udb_extra_f2_a->sue_rmat2 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat2(msg); - serial_udb_extra_f2_a->sue_rmat3 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat3(msg); - serial_udb_extra_f2_a->sue_rmat4 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat4(msg); - serial_udb_extra_f2_a->sue_rmat5 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat5(msg); - serial_udb_extra_f2_a->sue_rmat6 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat6(msg); - serial_udb_extra_f2_a->sue_rmat7 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat7(msg); - serial_udb_extra_f2_a->sue_rmat8 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat8(msg); - serial_udb_extra_f2_a->sue_cog = mavlink_msg_serial_udb_extra_f2_a_get_sue_cog(msg); - serial_udb_extra_f2_a->sue_sog = mavlink_msg_serial_udb_extra_f2_a_get_sue_sog(msg); - serial_udb_extra_f2_a->sue_cpu_load = mavlink_msg_serial_udb_extra_f2_a_get_sue_cpu_load(msg); - serial_udb_extra_f2_a->sue_voltage_milis = mavlink_msg_serial_udb_extra_f2_a_get_sue_voltage_milis(msg); - serial_udb_extra_f2_a->sue_air_speed_3DIMU = mavlink_msg_serial_udb_extra_f2_a_get_sue_air_speed_3DIMU(msg); - serial_udb_extra_f2_a->sue_estimated_wind_0 = mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_0(msg); - serial_udb_extra_f2_a->sue_estimated_wind_1 = mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_1(msg); - serial_udb_extra_f2_a->sue_estimated_wind_2 = mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_2(msg); - serial_udb_extra_f2_a->sue_magFieldEarth0 = mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth0(msg); - serial_udb_extra_f2_a->sue_magFieldEarth1 = mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth1(msg); - serial_udb_extra_f2_a->sue_magFieldEarth2 = mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth2(msg); - serial_udb_extra_f2_a->sue_svs = mavlink_msg_serial_udb_extra_f2_a_get_sue_svs(msg); - serial_udb_extra_f2_a->sue_hdop = mavlink_msg_serial_udb_extra_f2_a_get_sue_hdop(msg); - serial_udb_extra_f2_a->sue_status = mavlink_msg_serial_udb_extra_f2_a_get_sue_status(msg); -#else - memcpy(serial_udb_extra_f2_a, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h deleted file mode 100644 index 3e1757f74..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h +++ /dev/null @@ -1,977 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F2_B PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B 171 - -typedef struct __mavlink_serial_udb_extra_f2_b_t -{ - uint32_t sue_time; ///< Serial UDB Extra Time - uint32_t sue_flags; ///< Serial UDB Extra Status Flags - int16_t sue_pwm_input_1; ///< Serial UDB Extra PWM Input Channel 1 - int16_t sue_pwm_input_2; ///< Serial UDB Extra PWM Input Channel 2 - int16_t sue_pwm_input_3; ///< Serial UDB Extra PWM Input Channel 3 - int16_t sue_pwm_input_4; ///< Serial UDB Extra PWM Input Channel 4 - int16_t sue_pwm_input_5; ///< Serial UDB Extra PWM Input Channel 5 - int16_t sue_pwm_input_6; ///< Serial UDB Extra PWM Input Channel 6 - int16_t sue_pwm_input_7; ///< Serial UDB Extra PWM Input Channel 7 - int16_t sue_pwm_input_8; ///< Serial UDB Extra PWM Input Channel 8 - int16_t sue_pwm_input_9; ///< Serial UDB Extra PWM Input Channel 9 - int16_t sue_pwm_input_10; ///< Serial UDB Extra PWM Input Channel 10 - int16_t sue_pwm_output_1; ///< Serial UDB Extra PWM Output Channel 1 - int16_t sue_pwm_output_2; ///< Serial UDB Extra PWM Output Channel 2 - int16_t sue_pwm_output_3; ///< Serial UDB Extra PWM Output Channel 3 - int16_t sue_pwm_output_4; ///< Serial UDB Extra PWM Output Channel 4 - int16_t sue_pwm_output_5; ///< Serial UDB Extra PWM Output Channel 5 - int16_t sue_pwm_output_6; ///< Serial UDB Extra PWM Output Channel 6 - int16_t sue_pwm_output_7; ///< Serial UDB Extra PWM Output Channel 7 - int16_t sue_pwm_output_8; ///< Serial UDB Extra PWM Output Channel 8 - int16_t sue_pwm_output_9; ///< Serial UDB Extra PWM Output Channel 9 - int16_t sue_pwm_output_10; ///< Serial UDB Extra PWM Output Channel 10 - int16_t sue_imu_location_x; ///< Serial UDB Extra IMU Location X - int16_t sue_imu_location_y; ///< Serial UDB Extra IMU Location Y - int16_t sue_imu_location_z; ///< Serial UDB Extra IMU Location Z - int16_t sue_osc_fails; ///< Serial UDB Extra Oscillator Failure Count - int16_t sue_imu_velocity_x; ///< Serial UDB Extra IMU Velocity X - int16_t sue_imu_velocity_y; ///< Serial UDB Extra IMU Velocity Y - int16_t sue_imu_velocity_z; ///< Serial UDB Extra IMU Velocity Z - int16_t sue_waypoint_goal_x; ///< Serial UDB Extra Current Waypoint Goal X - int16_t sue_waypoint_goal_y; ///< Serial UDB Extra Current Waypoint Goal Y - int16_t sue_waypoint_goal_z; ///< Serial UDB Extra Current Waypoint Goal Z - int16_t sue_memory_stack_free; ///< Serial UDB Extra Stack Memory Free -} mavlink_serial_udb_extra_f2_b_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN 70 -#define MAVLINK_MSG_ID_171_LEN 70 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC 169 -#define MAVLINK_MSG_ID_171_CRC 169 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F2_B { \ - "SERIAL_UDB_EXTRA_F2_B", \ - 33, \ - { { "sue_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_time) }, \ - { "sue_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_flags) }, \ - { "sue_pwm_input_1", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_1) }, \ - { "sue_pwm_input_2", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_2) }, \ - { "sue_pwm_input_3", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_3) }, \ - { "sue_pwm_input_4", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_4) }, \ - { "sue_pwm_input_5", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_5) }, \ - { "sue_pwm_input_6", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_6) }, \ - { "sue_pwm_input_7", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_7) }, \ - { "sue_pwm_input_8", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_8) }, \ - { "sue_pwm_input_9", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_9) }, \ - { "sue_pwm_input_10", NULL, MAVLINK_TYPE_INT16_T, 0, 26, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_10) }, \ - { "sue_pwm_output_1", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_1) }, \ - { "sue_pwm_output_2", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_2) }, \ - { "sue_pwm_output_3", NULL, MAVLINK_TYPE_INT16_T, 0, 32, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_3) }, \ - { "sue_pwm_output_4", NULL, MAVLINK_TYPE_INT16_T, 0, 34, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_4) }, \ - { "sue_pwm_output_5", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_5) }, \ - { "sue_pwm_output_6", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_6) }, \ - { "sue_pwm_output_7", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_7) }, \ - { "sue_pwm_output_8", NULL, MAVLINK_TYPE_INT16_T, 0, 42, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_8) }, \ - { "sue_pwm_output_9", NULL, MAVLINK_TYPE_INT16_T, 0, 44, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_9) }, \ - { "sue_pwm_output_10", NULL, MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_10) }, \ - { "sue_imu_location_x", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_location_x) }, \ - { "sue_imu_location_y", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_location_y) }, \ - { "sue_imu_location_z", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_location_z) }, \ - { "sue_osc_fails", NULL, MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_osc_fails) }, \ - { "sue_imu_velocity_x", NULL, MAVLINK_TYPE_INT16_T, 0, 56, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_velocity_x) }, \ - { "sue_imu_velocity_y", NULL, MAVLINK_TYPE_INT16_T, 0, 58, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_velocity_y) }, \ - { "sue_imu_velocity_z", NULL, MAVLINK_TYPE_INT16_T, 0, 60, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_velocity_z) }, \ - { "sue_waypoint_goal_x", NULL, MAVLINK_TYPE_INT16_T, 0, 62, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_waypoint_goal_x) }, \ - { "sue_waypoint_goal_y", NULL, MAVLINK_TYPE_INT16_T, 0, 64, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_waypoint_goal_y) }, \ - { "sue_waypoint_goal_z", NULL, MAVLINK_TYPE_INT16_T, 0, 66, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_waypoint_goal_z) }, \ - { "sue_memory_stack_free", NULL, MAVLINK_TYPE_INT16_T, 0, 68, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_memory_stack_free) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f2_b message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_time Serial UDB Extra Time - * @param sue_pwm_input_1 Serial UDB Extra PWM Input Channel 1 - * @param sue_pwm_input_2 Serial UDB Extra PWM Input Channel 2 - * @param sue_pwm_input_3 Serial UDB Extra PWM Input Channel 3 - * @param sue_pwm_input_4 Serial UDB Extra PWM Input Channel 4 - * @param sue_pwm_input_5 Serial UDB Extra PWM Input Channel 5 - * @param sue_pwm_input_6 Serial UDB Extra PWM Input Channel 6 - * @param sue_pwm_input_7 Serial UDB Extra PWM Input Channel 7 - * @param sue_pwm_input_8 Serial UDB Extra PWM Input Channel 8 - * @param sue_pwm_input_9 Serial UDB Extra PWM Input Channel 9 - * @param sue_pwm_input_10 Serial UDB Extra PWM Input Channel 10 - * @param sue_pwm_output_1 Serial UDB Extra PWM Output Channel 1 - * @param sue_pwm_output_2 Serial UDB Extra PWM Output Channel 2 - * @param sue_pwm_output_3 Serial UDB Extra PWM Output Channel 3 - * @param sue_pwm_output_4 Serial UDB Extra PWM Output Channel 4 - * @param sue_pwm_output_5 Serial UDB Extra PWM Output Channel 5 - * @param sue_pwm_output_6 Serial UDB Extra PWM Output Channel 6 - * @param sue_pwm_output_7 Serial UDB Extra PWM Output Channel 7 - * @param sue_pwm_output_8 Serial UDB Extra PWM Output Channel 8 - * @param sue_pwm_output_9 Serial UDB Extra PWM Output Channel 9 - * @param sue_pwm_output_10 Serial UDB Extra PWM Output Channel 10 - * @param sue_imu_location_x Serial UDB Extra IMU Location X - * @param sue_imu_location_y Serial UDB Extra IMU Location Y - * @param sue_imu_location_z Serial UDB Extra IMU Location Z - * @param sue_flags Serial UDB Extra Status Flags - * @param sue_osc_fails Serial UDB Extra Oscillator Failure Count - * @param sue_imu_velocity_x Serial UDB Extra IMU Velocity X - * @param sue_imu_velocity_y Serial UDB Extra IMU Velocity Y - * @param sue_imu_velocity_z Serial UDB Extra IMU Velocity Z - * @param sue_waypoint_goal_x Serial UDB Extra Current Waypoint Goal X - * @param sue_waypoint_goal_y Serial UDB Extra Current Waypoint Goal Y - * @param sue_waypoint_goal_z Serial UDB Extra Current Waypoint Goal Z - * @param sue_memory_stack_free Serial UDB Extra Stack Memory Free - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t sue_time, int16_t sue_pwm_input_1, int16_t sue_pwm_input_2, int16_t sue_pwm_input_3, int16_t sue_pwm_input_4, int16_t sue_pwm_input_5, int16_t sue_pwm_input_6, int16_t sue_pwm_input_7, int16_t sue_pwm_input_8, int16_t sue_pwm_input_9, int16_t sue_pwm_input_10, int16_t sue_pwm_output_1, int16_t sue_pwm_output_2, int16_t sue_pwm_output_3, int16_t sue_pwm_output_4, int16_t sue_pwm_output_5, int16_t sue_pwm_output_6, int16_t sue_pwm_output_7, int16_t sue_pwm_output_8, int16_t sue_pwm_output_9, int16_t sue_pwm_output_10, int16_t sue_imu_location_x, int16_t sue_imu_location_y, int16_t sue_imu_location_z, uint32_t sue_flags, int16_t sue_osc_fails, int16_t sue_imu_velocity_x, int16_t sue_imu_velocity_y, int16_t sue_imu_velocity_z, int16_t sue_waypoint_goal_x, int16_t sue_waypoint_goal_y, int16_t sue_waypoint_goal_z, int16_t sue_memory_stack_free) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN]; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_uint32_t(buf, 4, sue_flags); - _mav_put_int16_t(buf, 8, sue_pwm_input_1); - _mav_put_int16_t(buf, 10, sue_pwm_input_2); - _mav_put_int16_t(buf, 12, sue_pwm_input_3); - _mav_put_int16_t(buf, 14, sue_pwm_input_4); - _mav_put_int16_t(buf, 16, sue_pwm_input_5); - _mav_put_int16_t(buf, 18, sue_pwm_input_6); - _mav_put_int16_t(buf, 20, sue_pwm_input_7); - _mav_put_int16_t(buf, 22, sue_pwm_input_8); - _mav_put_int16_t(buf, 24, sue_pwm_input_9); - _mav_put_int16_t(buf, 26, sue_pwm_input_10); - _mav_put_int16_t(buf, 28, sue_pwm_output_1); - _mav_put_int16_t(buf, 30, sue_pwm_output_2); - _mav_put_int16_t(buf, 32, sue_pwm_output_3); - _mav_put_int16_t(buf, 34, sue_pwm_output_4); - _mav_put_int16_t(buf, 36, sue_pwm_output_5); - _mav_put_int16_t(buf, 38, sue_pwm_output_6); - _mav_put_int16_t(buf, 40, sue_pwm_output_7); - _mav_put_int16_t(buf, 42, sue_pwm_output_8); - _mav_put_int16_t(buf, 44, sue_pwm_output_9); - _mav_put_int16_t(buf, 46, sue_pwm_output_10); - _mav_put_int16_t(buf, 48, sue_imu_location_x); - _mav_put_int16_t(buf, 50, sue_imu_location_y); - _mav_put_int16_t(buf, 52, sue_imu_location_z); - _mav_put_int16_t(buf, 54, sue_osc_fails); - _mav_put_int16_t(buf, 56, sue_imu_velocity_x); - _mav_put_int16_t(buf, 58, sue_imu_velocity_y); - _mav_put_int16_t(buf, 60, sue_imu_velocity_z); - _mav_put_int16_t(buf, 62, sue_waypoint_goal_x); - _mav_put_int16_t(buf, 64, sue_waypoint_goal_y); - _mav_put_int16_t(buf, 66, sue_waypoint_goal_z); - _mav_put_int16_t(buf, 68, sue_memory_stack_free); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#else - mavlink_serial_udb_extra_f2_b_t packet; - packet.sue_time = sue_time; - packet.sue_flags = sue_flags; - packet.sue_pwm_input_1 = sue_pwm_input_1; - packet.sue_pwm_input_2 = sue_pwm_input_2; - packet.sue_pwm_input_3 = sue_pwm_input_3; - packet.sue_pwm_input_4 = sue_pwm_input_4; - packet.sue_pwm_input_5 = sue_pwm_input_5; - packet.sue_pwm_input_6 = sue_pwm_input_6; - packet.sue_pwm_input_7 = sue_pwm_input_7; - packet.sue_pwm_input_8 = sue_pwm_input_8; - packet.sue_pwm_input_9 = sue_pwm_input_9; - packet.sue_pwm_input_10 = sue_pwm_input_10; - packet.sue_pwm_output_1 = sue_pwm_output_1; - packet.sue_pwm_output_2 = sue_pwm_output_2; - packet.sue_pwm_output_3 = sue_pwm_output_3; - packet.sue_pwm_output_4 = sue_pwm_output_4; - packet.sue_pwm_output_5 = sue_pwm_output_5; - packet.sue_pwm_output_6 = sue_pwm_output_6; - packet.sue_pwm_output_7 = sue_pwm_output_7; - packet.sue_pwm_output_8 = sue_pwm_output_8; - packet.sue_pwm_output_9 = sue_pwm_output_9; - packet.sue_pwm_output_10 = sue_pwm_output_10; - packet.sue_imu_location_x = sue_imu_location_x; - packet.sue_imu_location_y = sue_imu_location_y; - packet.sue_imu_location_z = sue_imu_location_z; - packet.sue_osc_fails = sue_osc_fails; - packet.sue_imu_velocity_x = sue_imu_velocity_x; - packet.sue_imu_velocity_y = sue_imu_velocity_y; - packet.sue_imu_velocity_z = sue_imu_velocity_z; - packet.sue_waypoint_goal_x = sue_waypoint_goal_x; - packet.sue_waypoint_goal_y = sue_waypoint_goal_y; - packet.sue_waypoint_goal_z = sue_waypoint_goal_z; - packet.sue_memory_stack_free = sue_memory_stack_free; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f2_b message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_time Serial UDB Extra Time - * @param sue_pwm_input_1 Serial UDB Extra PWM Input Channel 1 - * @param sue_pwm_input_2 Serial UDB Extra PWM Input Channel 2 - * @param sue_pwm_input_3 Serial UDB Extra PWM Input Channel 3 - * @param sue_pwm_input_4 Serial UDB Extra PWM Input Channel 4 - * @param sue_pwm_input_5 Serial UDB Extra PWM Input Channel 5 - * @param sue_pwm_input_6 Serial UDB Extra PWM Input Channel 6 - * @param sue_pwm_input_7 Serial UDB Extra PWM Input Channel 7 - * @param sue_pwm_input_8 Serial UDB Extra PWM Input Channel 8 - * @param sue_pwm_input_9 Serial UDB Extra PWM Input Channel 9 - * @param sue_pwm_input_10 Serial UDB Extra PWM Input Channel 10 - * @param sue_pwm_output_1 Serial UDB Extra PWM Output Channel 1 - * @param sue_pwm_output_2 Serial UDB Extra PWM Output Channel 2 - * @param sue_pwm_output_3 Serial UDB Extra PWM Output Channel 3 - * @param sue_pwm_output_4 Serial UDB Extra PWM Output Channel 4 - * @param sue_pwm_output_5 Serial UDB Extra PWM Output Channel 5 - * @param sue_pwm_output_6 Serial UDB Extra PWM Output Channel 6 - * @param sue_pwm_output_7 Serial UDB Extra PWM Output Channel 7 - * @param sue_pwm_output_8 Serial UDB Extra PWM Output Channel 8 - * @param sue_pwm_output_9 Serial UDB Extra PWM Output Channel 9 - * @param sue_pwm_output_10 Serial UDB Extra PWM Output Channel 10 - * @param sue_imu_location_x Serial UDB Extra IMU Location X - * @param sue_imu_location_y Serial UDB Extra IMU Location Y - * @param sue_imu_location_z Serial UDB Extra IMU Location Z - * @param sue_flags Serial UDB Extra Status Flags - * @param sue_osc_fails Serial UDB Extra Oscillator Failure Count - * @param sue_imu_velocity_x Serial UDB Extra IMU Velocity X - * @param sue_imu_velocity_y Serial UDB Extra IMU Velocity Y - * @param sue_imu_velocity_z Serial UDB Extra IMU Velocity Z - * @param sue_waypoint_goal_x Serial UDB Extra Current Waypoint Goal X - * @param sue_waypoint_goal_y Serial UDB Extra Current Waypoint Goal Y - * @param sue_waypoint_goal_z Serial UDB Extra Current Waypoint Goal Z - * @param sue_memory_stack_free Serial UDB Extra Stack Memory Free - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t sue_time,int16_t sue_pwm_input_1,int16_t sue_pwm_input_2,int16_t sue_pwm_input_3,int16_t sue_pwm_input_4,int16_t sue_pwm_input_5,int16_t sue_pwm_input_6,int16_t sue_pwm_input_7,int16_t sue_pwm_input_8,int16_t sue_pwm_input_9,int16_t sue_pwm_input_10,int16_t sue_pwm_output_1,int16_t sue_pwm_output_2,int16_t sue_pwm_output_3,int16_t sue_pwm_output_4,int16_t sue_pwm_output_5,int16_t sue_pwm_output_6,int16_t sue_pwm_output_7,int16_t sue_pwm_output_8,int16_t sue_pwm_output_9,int16_t sue_pwm_output_10,int16_t sue_imu_location_x,int16_t sue_imu_location_y,int16_t sue_imu_location_z,uint32_t sue_flags,int16_t sue_osc_fails,int16_t sue_imu_velocity_x,int16_t sue_imu_velocity_y,int16_t sue_imu_velocity_z,int16_t sue_waypoint_goal_x,int16_t sue_waypoint_goal_y,int16_t sue_waypoint_goal_z,int16_t sue_memory_stack_free) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN]; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_uint32_t(buf, 4, sue_flags); - _mav_put_int16_t(buf, 8, sue_pwm_input_1); - _mav_put_int16_t(buf, 10, sue_pwm_input_2); - _mav_put_int16_t(buf, 12, sue_pwm_input_3); - _mav_put_int16_t(buf, 14, sue_pwm_input_4); - _mav_put_int16_t(buf, 16, sue_pwm_input_5); - _mav_put_int16_t(buf, 18, sue_pwm_input_6); - _mav_put_int16_t(buf, 20, sue_pwm_input_7); - _mav_put_int16_t(buf, 22, sue_pwm_input_8); - _mav_put_int16_t(buf, 24, sue_pwm_input_9); - _mav_put_int16_t(buf, 26, sue_pwm_input_10); - _mav_put_int16_t(buf, 28, sue_pwm_output_1); - _mav_put_int16_t(buf, 30, sue_pwm_output_2); - _mav_put_int16_t(buf, 32, sue_pwm_output_3); - _mav_put_int16_t(buf, 34, sue_pwm_output_4); - _mav_put_int16_t(buf, 36, sue_pwm_output_5); - _mav_put_int16_t(buf, 38, sue_pwm_output_6); - _mav_put_int16_t(buf, 40, sue_pwm_output_7); - _mav_put_int16_t(buf, 42, sue_pwm_output_8); - _mav_put_int16_t(buf, 44, sue_pwm_output_9); - _mav_put_int16_t(buf, 46, sue_pwm_output_10); - _mav_put_int16_t(buf, 48, sue_imu_location_x); - _mav_put_int16_t(buf, 50, sue_imu_location_y); - _mav_put_int16_t(buf, 52, sue_imu_location_z); - _mav_put_int16_t(buf, 54, sue_osc_fails); - _mav_put_int16_t(buf, 56, sue_imu_velocity_x); - _mav_put_int16_t(buf, 58, sue_imu_velocity_y); - _mav_put_int16_t(buf, 60, sue_imu_velocity_z); - _mav_put_int16_t(buf, 62, sue_waypoint_goal_x); - _mav_put_int16_t(buf, 64, sue_waypoint_goal_y); - _mav_put_int16_t(buf, 66, sue_waypoint_goal_z); - _mav_put_int16_t(buf, 68, sue_memory_stack_free); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#else - mavlink_serial_udb_extra_f2_b_t packet; - packet.sue_time = sue_time; - packet.sue_flags = sue_flags; - packet.sue_pwm_input_1 = sue_pwm_input_1; - packet.sue_pwm_input_2 = sue_pwm_input_2; - packet.sue_pwm_input_3 = sue_pwm_input_3; - packet.sue_pwm_input_4 = sue_pwm_input_4; - packet.sue_pwm_input_5 = sue_pwm_input_5; - packet.sue_pwm_input_6 = sue_pwm_input_6; - packet.sue_pwm_input_7 = sue_pwm_input_7; - packet.sue_pwm_input_8 = sue_pwm_input_8; - packet.sue_pwm_input_9 = sue_pwm_input_9; - packet.sue_pwm_input_10 = sue_pwm_input_10; - packet.sue_pwm_output_1 = sue_pwm_output_1; - packet.sue_pwm_output_2 = sue_pwm_output_2; - packet.sue_pwm_output_3 = sue_pwm_output_3; - packet.sue_pwm_output_4 = sue_pwm_output_4; - packet.sue_pwm_output_5 = sue_pwm_output_5; - packet.sue_pwm_output_6 = sue_pwm_output_6; - packet.sue_pwm_output_7 = sue_pwm_output_7; - packet.sue_pwm_output_8 = sue_pwm_output_8; - packet.sue_pwm_output_9 = sue_pwm_output_9; - packet.sue_pwm_output_10 = sue_pwm_output_10; - packet.sue_imu_location_x = sue_imu_location_x; - packet.sue_imu_location_y = sue_imu_location_y; - packet.sue_imu_location_z = sue_imu_location_z; - packet.sue_osc_fails = sue_osc_fails; - packet.sue_imu_velocity_x = sue_imu_velocity_x; - packet.sue_imu_velocity_y = sue_imu_velocity_y; - packet.sue_imu_velocity_z = sue_imu_velocity_z; - packet.sue_waypoint_goal_x = sue_waypoint_goal_x; - packet.sue_waypoint_goal_y = sue_waypoint_goal_y; - packet.sue_waypoint_goal_z = sue_waypoint_goal_z; - packet.sue_memory_stack_free = sue_memory_stack_free; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f2_b struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f2_b C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_b_t* serial_udb_extra_f2_b) -{ - return mavlink_msg_serial_udb_extra_f2_b_pack(system_id, component_id, msg, serial_udb_extra_f2_b->sue_time, serial_udb_extra_f2_b->sue_pwm_input_1, serial_udb_extra_f2_b->sue_pwm_input_2, serial_udb_extra_f2_b->sue_pwm_input_3, serial_udb_extra_f2_b->sue_pwm_input_4, serial_udb_extra_f2_b->sue_pwm_input_5, serial_udb_extra_f2_b->sue_pwm_input_6, serial_udb_extra_f2_b->sue_pwm_input_7, serial_udb_extra_f2_b->sue_pwm_input_8, serial_udb_extra_f2_b->sue_pwm_input_9, serial_udb_extra_f2_b->sue_pwm_input_10, serial_udb_extra_f2_b->sue_pwm_output_1, serial_udb_extra_f2_b->sue_pwm_output_2, serial_udb_extra_f2_b->sue_pwm_output_3, serial_udb_extra_f2_b->sue_pwm_output_4, serial_udb_extra_f2_b->sue_pwm_output_5, serial_udb_extra_f2_b->sue_pwm_output_6, serial_udb_extra_f2_b->sue_pwm_output_7, serial_udb_extra_f2_b->sue_pwm_output_8, serial_udb_extra_f2_b->sue_pwm_output_9, serial_udb_extra_f2_b->sue_pwm_output_10, serial_udb_extra_f2_b->sue_imu_location_x, serial_udb_extra_f2_b->sue_imu_location_y, serial_udb_extra_f2_b->sue_imu_location_z, serial_udb_extra_f2_b->sue_flags, serial_udb_extra_f2_b->sue_osc_fails, serial_udb_extra_f2_b->sue_imu_velocity_x, serial_udb_extra_f2_b->sue_imu_velocity_y, serial_udb_extra_f2_b->sue_imu_velocity_z, serial_udb_extra_f2_b->sue_waypoint_goal_x, serial_udb_extra_f2_b->sue_waypoint_goal_y, serial_udb_extra_f2_b->sue_waypoint_goal_z, serial_udb_extra_f2_b->sue_memory_stack_free); -} - -/** - * @brief Encode a serial_udb_extra_f2_b struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f2_b C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_b_t* serial_udb_extra_f2_b) -{ - return mavlink_msg_serial_udb_extra_f2_b_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f2_b->sue_time, serial_udb_extra_f2_b->sue_pwm_input_1, serial_udb_extra_f2_b->sue_pwm_input_2, serial_udb_extra_f2_b->sue_pwm_input_3, serial_udb_extra_f2_b->sue_pwm_input_4, serial_udb_extra_f2_b->sue_pwm_input_5, serial_udb_extra_f2_b->sue_pwm_input_6, serial_udb_extra_f2_b->sue_pwm_input_7, serial_udb_extra_f2_b->sue_pwm_input_8, serial_udb_extra_f2_b->sue_pwm_input_9, serial_udb_extra_f2_b->sue_pwm_input_10, serial_udb_extra_f2_b->sue_pwm_output_1, serial_udb_extra_f2_b->sue_pwm_output_2, serial_udb_extra_f2_b->sue_pwm_output_3, serial_udb_extra_f2_b->sue_pwm_output_4, serial_udb_extra_f2_b->sue_pwm_output_5, serial_udb_extra_f2_b->sue_pwm_output_6, serial_udb_extra_f2_b->sue_pwm_output_7, serial_udb_extra_f2_b->sue_pwm_output_8, serial_udb_extra_f2_b->sue_pwm_output_9, serial_udb_extra_f2_b->sue_pwm_output_10, serial_udb_extra_f2_b->sue_imu_location_x, serial_udb_extra_f2_b->sue_imu_location_y, serial_udb_extra_f2_b->sue_imu_location_z, serial_udb_extra_f2_b->sue_flags, serial_udb_extra_f2_b->sue_osc_fails, serial_udb_extra_f2_b->sue_imu_velocity_x, serial_udb_extra_f2_b->sue_imu_velocity_y, serial_udb_extra_f2_b->sue_imu_velocity_z, serial_udb_extra_f2_b->sue_waypoint_goal_x, serial_udb_extra_f2_b->sue_waypoint_goal_y, serial_udb_extra_f2_b->sue_waypoint_goal_z, serial_udb_extra_f2_b->sue_memory_stack_free); -} - -/** - * @brief Send a serial_udb_extra_f2_b message - * @param chan MAVLink channel to send the message - * - * @param sue_time Serial UDB Extra Time - * @param sue_pwm_input_1 Serial UDB Extra PWM Input Channel 1 - * @param sue_pwm_input_2 Serial UDB Extra PWM Input Channel 2 - * @param sue_pwm_input_3 Serial UDB Extra PWM Input Channel 3 - * @param sue_pwm_input_4 Serial UDB Extra PWM Input Channel 4 - * @param sue_pwm_input_5 Serial UDB Extra PWM Input Channel 5 - * @param sue_pwm_input_6 Serial UDB Extra PWM Input Channel 6 - * @param sue_pwm_input_7 Serial UDB Extra PWM Input Channel 7 - * @param sue_pwm_input_8 Serial UDB Extra PWM Input Channel 8 - * @param sue_pwm_input_9 Serial UDB Extra PWM Input Channel 9 - * @param sue_pwm_input_10 Serial UDB Extra PWM Input Channel 10 - * @param sue_pwm_output_1 Serial UDB Extra PWM Output Channel 1 - * @param sue_pwm_output_2 Serial UDB Extra PWM Output Channel 2 - * @param sue_pwm_output_3 Serial UDB Extra PWM Output Channel 3 - * @param sue_pwm_output_4 Serial UDB Extra PWM Output Channel 4 - * @param sue_pwm_output_5 Serial UDB Extra PWM Output Channel 5 - * @param sue_pwm_output_6 Serial UDB Extra PWM Output Channel 6 - * @param sue_pwm_output_7 Serial UDB Extra PWM Output Channel 7 - * @param sue_pwm_output_8 Serial UDB Extra PWM Output Channel 8 - * @param sue_pwm_output_9 Serial UDB Extra PWM Output Channel 9 - * @param sue_pwm_output_10 Serial UDB Extra PWM Output Channel 10 - * @param sue_imu_location_x Serial UDB Extra IMU Location X - * @param sue_imu_location_y Serial UDB Extra IMU Location Y - * @param sue_imu_location_z Serial UDB Extra IMU Location Z - * @param sue_flags Serial UDB Extra Status Flags - * @param sue_osc_fails Serial UDB Extra Oscillator Failure Count - * @param sue_imu_velocity_x Serial UDB Extra IMU Velocity X - * @param sue_imu_velocity_y Serial UDB Extra IMU Velocity Y - * @param sue_imu_velocity_z Serial UDB Extra IMU Velocity Z - * @param sue_waypoint_goal_x Serial UDB Extra Current Waypoint Goal X - * @param sue_waypoint_goal_y Serial UDB Extra Current Waypoint Goal Y - * @param sue_waypoint_goal_z Serial UDB Extra Current Waypoint Goal Z - * @param sue_memory_stack_free Serial UDB Extra Stack Memory Free - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f2_b_send(mavlink_channel_t chan, uint32_t sue_time, int16_t sue_pwm_input_1, int16_t sue_pwm_input_2, int16_t sue_pwm_input_3, int16_t sue_pwm_input_4, int16_t sue_pwm_input_5, int16_t sue_pwm_input_6, int16_t sue_pwm_input_7, int16_t sue_pwm_input_8, int16_t sue_pwm_input_9, int16_t sue_pwm_input_10, int16_t sue_pwm_output_1, int16_t sue_pwm_output_2, int16_t sue_pwm_output_3, int16_t sue_pwm_output_4, int16_t sue_pwm_output_5, int16_t sue_pwm_output_6, int16_t sue_pwm_output_7, int16_t sue_pwm_output_8, int16_t sue_pwm_output_9, int16_t sue_pwm_output_10, int16_t sue_imu_location_x, int16_t sue_imu_location_y, int16_t sue_imu_location_z, uint32_t sue_flags, int16_t sue_osc_fails, int16_t sue_imu_velocity_x, int16_t sue_imu_velocity_y, int16_t sue_imu_velocity_z, int16_t sue_waypoint_goal_x, int16_t sue_waypoint_goal_y, int16_t sue_waypoint_goal_z, int16_t sue_memory_stack_free) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN]; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_uint32_t(buf, 4, sue_flags); - _mav_put_int16_t(buf, 8, sue_pwm_input_1); - _mav_put_int16_t(buf, 10, sue_pwm_input_2); - _mav_put_int16_t(buf, 12, sue_pwm_input_3); - _mav_put_int16_t(buf, 14, sue_pwm_input_4); - _mav_put_int16_t(buf, 16, sue_pwm_input_5); - _mav_put_int16_t(buf, 18, sue_pwm_input_6); - _mav_put_int16_t(buf, 20, sue_pwm_input_7); - _mav_put_int16_t(buf, 22, sue_pwm_input_8); - _mav_put_int16_t(buf, 24, sue_pwm_input_9); - _mav_put_int16_t(buf, 26, sue_pwm_input_10); - _mav_put_int16_t(buf, 28, sue_pwm_output_1); - _mav_put_int16_t(buf, 30, sue_pwm_output_2); - _mav_put_int16_t(buf, 32, sue_pwm_output_3); - _mav_put_int16_t(buf, 34, sue_pwm_output_4); - _mav_put_int16_t(buf, 36, sue_pwm_output_5); - _mav_put_int16_t(buf, 38, sue_pwm_output_6); - _mav_put_int16_t(buf, 40, sue_pwm_output_7); - _mav_put_int16_t(buf, 42, sue_pwm_output_8); - _mav_put_int16_t(buf, 44, sue_pwm_output_9); - _mav_put_int16_t(buf, 46, sue_pwm_output_10); - _mav_put_int16_t(buf, 48, sue_imu_location_x); - _mav_put_int16_t(buf, 50, sue_imu_location_y); - _mav_put_int16_t(buf, 52, sue_imu_location_z); - _mav_put_int16_t(buf, 54, sue_osc_fails); - _mav_put_int16_t(buf, 56, sue_imu_velocity_x); - _mav_put_int16_t(buf, 58, sue_imu_velocity_y); - _mav_put_int16_t(buf, 60, sue_imu_velocity_z); - _mav_put_int16_t(buf, 62, sue_waypoint_goal_x); - _mav_put_int16_t(buf, 64, sue_waypoint_goal_y); - _mav_put_int16_t(buf, 66, sue_waypoint_goal_z); - _mav_put_int16_t(buf, 68, sue_memory_stack_free); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif -#else - mavlink_serial_udb_extra_f2_b_t packet; - packet.sue_time = sue_time; - packet.sue_flags = sue_flags; - packet.sue_pwm_input_1 = sue_pwm_input_1; - packet.sue_pwm_input_2 = sue_pwm_input_2; - packet.sue_pwm_input_3 = sue_pwm_input_3; - packet.sue_pwm_input_4 = sue_pwm_input_4; - packet.sue_pwm_input_5 = sue_pwm_input_5; - packet.sue_pwm_input_6 = sue_pwm_input_6; - packet.sue_pwm_input_7 = sue_pwm_input_7; - packet.sue_pwm_input_8 = sue_pwm_input_8; - packet.sue_pwm_input_9 = sue_pwm_input_9; - packet.sue_pwm_input_10 = sue_pwm_input_10; - packet.sue_pwm_output_1 = sue_pwm_output_1; - packet.sue_pwm_output_2 = sue_pwm_output_2; - packet.sue_pwm_output_3 = sue_pwm_output_3; - packet.sue_pwm_output_4 = sue_pwm_output_4; - packet.sue_pwm_output_5 = sue_pwm_output_5; - packet.sue_pwm_output_6 = sue_pwm_output_6; - packet.sue_pwm_output_7 = sue_pwm_output_7; - packet.sue_pwm_output_8 = sue_pwm_output_8; - packet.sue_pwm_output_9 = sue_pwm_output_9; - packet.sue_pwm_output_10 = sue_pwm_output_10; - packet.sue_imu_location_x = sue_imu_location_x; - packet.sue_imu_location_y = sue_imu_location_y; - packet.sue_imu_location_z = sue_imu_location_z; - packet.sue_osc_fails = sue_osc_fails; - packet.sue_imu_velocity_x = sue_imu_velocity_x; - packet.sue_imu_velocity_y = sue_imu_velocity_y; - packet.sue_imu_velocity_z = sue_imu_velocity_z; - packet.sue_waypoint_goal_x = sue_waypoint_goal_x; - packet.sue_waypoint_goal_y = sue_waypoint_goal_y; - packet.sue_waypoint_goal_z = sue_waypoint_goal_z; - packet.sue_memory_stack_free = sue_memory_stack_free; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_serial_udb_extra_f2_b_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t sue_time, int16_t sue_pwm_input_1, int16_t sue_pwm_input_2, int16_t sue_pwm_input_3, int16_t sue_pwm_input_4, int16_t sue_pwm_input_5, int16_t sue_pwm_input_6, int16_t sue_pwm_input_7, int16_t sue_pwm_input_8, int16_t sue_pwm_input_9, int16_t sue_pwm_input_10, int16_t sue_pwm_output_1, int16_t sue_pwm_output_2, int16_t sue_pwm_output_3, int16_t sue_pwm_output_4, int16_t sue_pwm_output_5, int16_t sue_pwm_output_6, int16_t sue_pwm_output_7, int16_t sue_pwm_output_8, int16_t sue_pwm_output_9, int16_t sue_pwm_output_10, int16_t sue_imu_location_x, int16_t sue_imu_location_y, int16_t sue_imu_location_z, uint32_t sue_flags, int16_t sue_osc_fails, int16_t sue_imu_velocity_x, int16_t sue_imu_velocity_y, int16_t sue_imu_velocity_z, int16_t sue_waypoint_goal_x, int16_t sue_waypoint_goal_y, int16_t sue_waypoint_goal_z, int16_t sue_memory_stack_free) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, sue_time); - _mav_put_uint32_t(buf, 4, sue_flags); - _mav_put_int16_t(buf, 8, sue_pwm_input_1); - _mav_put_int16_t(buf, 10, sue_pwm_input_2); - _mav_put_int16_t(buf, 12, sue_pwm_input_3); - _mav_put_int16_t(buf, 14, sue_pwm_input_4); - _mav_put_int16_t(buf, 16, sue_pwm_input_5); - _mav_put_int16_t(buf, 18, sue_pwm_input_6); - _mav_put_int16_t(buf, 20, sue_pwm_input_7); - _mav_put_int16_t(buf, 22, sue_pwm_input_8); - _mav_put_int16_t(buf, 24, sue_pwm_input_9); - _mav_put_int16_t(buf, 26, sue_pwm_input_10); - _mav_put_int16_t(buf, 28, sue_pwm_output_1); - _mav_put_int16_t(buf, 30, sue_pwm_output_2); - _mav_put_int16_t(buf, 32, sue_pwm_output_3); - _mav_put_int16_t(buf, 34, sue_pwm_output_4); - _mav_put_int16_t(buf, 36, sue_pwm_output_5); - _mav_put_int16_t(buf, 38, sue_pwm_output_6); - _mav_put_int16_t(buf, 40, sue_pwm_output_7); - _mav_put_int16_t(buf, 42, sue_pwm_output_8); - _mav_put_int16_t(buf, 44, sue_pwm_output_9); - _mav_put_int16_t(buf, 46, sue_pwm_output_10); - _mav_put_int16_t(buf, 48, sue_imu_location_x); - _mav_put_int16_t(buf, 50, sue_imu_location_y); - _mav_put_int16_t(buf, 52, sue_imu_location_z); - _mav_put_int16_t(buf, 54, sue_osc_fails); - _mav_put_int16_t(buf, 56, sue_imu_velocity_x); - _mav_put_int16_t(buf, 58, sue_imu_velocity_y); - _mav_put_int16_t(buf, 60, sue_imu_velocity_z); - _mav_put_int16_t(buf, 62, sue_waypoint_goal_x); - _mav_put_int16_t(buf, 64, sue_waypoint_goal_y); - _mav_put_int16_t(buf, 66, sue_waypoint_goal_z); - _mav_put_int16_t(buf, 68, sue_memory_stack_free); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif -#else - mavlink_serial_udb_extra_f2_b_t *packet = (mavlink_serial_udb_extra_f2_b_t *)msgbuf; - packet->sue_time = sue_time; - packet->sue_flags = sue_flags; - packet->sue_pwm_input_1 = sue_pwm_input_1; - packet->sue_pwm_input_2 = sue_pwm_input_2; - packet->sue_pwm_input_3 = sue_pwm_input_3; - packet->sue_pwm_input_4 = sue_pwm_input_4; - packet->sue_pwm_input_5 = sue_pwm_input_5; - packet->sue_pwm_input_6 = sue_pwm_input_6; - packet->sue_pwm_input_7 = sue_pwm_input_7; - packet->sue_pwm_input_8 = sue_pwm_input_8; - packet->sue_pwm_input_9 = sue_pwm_input_9; - packet->sue_pwm_input_10 = sue_pwm_input_10; - packet->sue_pwm_output_1 = sue_pwm_output_1; - packet->sue_pwm_output_2 = sue_pwm_output_2; - packet->sue_pwm_output_3 = sue_pwm_output_3; - packet->sue_pwm_output_4 = sue_pwm_output_4; - packet->sue_pwm_output_5 = sue_pwm_output_5; - packet->sue_pwm_output_6 = sue_pwm_output_6; - packet->sue_pwm_output_7 = sue_pwm_output_7; - packet->sue_pwm_output_8 = sue_pwm_output_8; - packet->sue_pwm_output_9 = sue_pwm_output_9; - packet->sue_pwm_output_10 = sue_pwm_output_10; - packet->sue_imu_location_x = sue_imu_location_x; - packet->sue_imu_location_y = sue_imu_location_y; - packet->sue_imu_location_z = sue_imu_location_z; - packet->sue_osc_fails = sue_osc_fails; - packet->sue_imu_velocity_x = sue_imu_velocity_x; - packet->sue_imu_velocity_y = sue_imu_velocity_y; - packet->sue_imu_velocity_z = sue_imu_velocity_z; - packet->sue_waypoint_goal_x = sue_waypoint_goal_x; - packet->sue_waypoint_goal_y = sue_waypoint_goal_y; - packet->sue_waypoint_goal_z = sue_waypoint_goal_z; - packet->sue_memory_stack_free = sue_memory_stack_free; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F2_B UNPACKING - - -/** - * @brief Get field sue_time from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Time - */ -static inline uint32_t mavlink_msg_serial_udb_extra_f2_b_get_sue_time(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field sue_pwm_input_1 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 1 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 8); -} - -/** - * @brief Get field sue_pwm_input_2 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 2 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 10); -} - -/** - * @brief Get field sue_pwm_input_3 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 3 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 12); -} - -/** - * @brief Get field sue_pwm_input_4 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 4 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 14); -} - -/** - * @brief Get field sue_pwm_input_5 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 5 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 16); -} - -/** - * @brief Get field sue_pwm_input_6 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 6 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 18); -} - -/** - * @brief Get field sue_pwm_input_7 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 7 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Get field sue_pwm_input_8 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 8 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 22); -} - -/** - * @brief Get field sue_pwm_input_9 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 9 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_9(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 24); -} - -/** - * @brief Get field sue_pwm_input_10 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Input Channel 10 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_10(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 26); -} - -/** - * @brief Get field sue_pwm_output_1 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 1 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 28); -} - -/** - * @brief Get field sue_pwm_output_2 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 2 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 30); -} - -/** - * @brief Get field sue_pwm_output_3 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 3 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 32); -} - -/** - * @brief Get field sue_pwm_output_4 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 4 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 34); -} - -/** - * @brief Get field sue_pwm_output_5 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 5 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 36); -} - -/** - * @brief Get field sue_pwm_output_6 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 6 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 38); -} - -/** - * @brief Get field sue_pwm_output_7 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 7 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 40); -} - -/** - * @brief Get field sue_pwm_output_8 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 8 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 42); -} - -/** - * @brief Get field sue_pwm_output_9 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 9 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_9(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 44); -} - -/** - * @brief Get field sue_pwm_output_10 from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra PWM Output Channel 10 - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_10(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 46); -} - -/** - * @brief Get field sue_imu_location_x from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra IMU Location X - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 48); -} - -/** - * @brief Get field sue_imu_location_y from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra IMU Location Y - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 50); -} - -/** - * @brief Get field sue_imu_location_z from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra IMU Location Z - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 52); -} - -/** - * @brief Get field sue_flags from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Status Flags - */ -static inline uint32_t mavlink_msg_serial_udb_extra_f2_b_get_sue_flags(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 4); -} - -/** - * @brief Get field sue_osc_fails from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Oscillator Failure Count - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_osc_fails(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 54); -} - -/** - * @brief Get field sue_imu_velocity_x from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra IMU Velocity X - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 56); -} - -/** - * @brief Get field sue_imu_velocity_y from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra IMU Velocity Y - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 58); -} - -/** - * @brief Get field sue_imu_velocity_z from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra IMU Velocity Z - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 60); -} - -/** - * @brief Get field sue_waypoint_goal_x from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Current Waypoint Goal X - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 62); -} - -/** - * @brief Get field sue_waypoint_goal_y from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Current Waypoint Goal Y - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 64); -} - -/** - * @brief Get field sue_waypoint_goal_z from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Current Waypoint Goal Z - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 66); -} - -/** - * @brief Get field sue_memory_stack_free from serial_udb_extra_f2_b message - * - * @return Serial UDB Extra Stack Memory Free - */ -static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_memory_stack_free(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 68); -} - -/** - * @brief Decode a serial_udb_extra_f2_b message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f2_b C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f2_b_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f2_b_t* serial_udb_extra_f2_b) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f2_b->sue_time = mavlink_msg_serial_udb_extra_f2_b_get_sue_time(msg); - serial_udb_extra_f2_b->sue_flags = mavlink_msg_serial_udb_extra_f2_b_get_sue_flags(msg); - serial_udb_extra_f2_b->sue_pwm_input_1 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_1(msg); - serial_udb_extra_f2_b->sue_pwm_input_2 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_2(msg); - serial_udb_extra_f2_b->sue_pwm_input_3 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_3(msg); - serial_udb_extra_f2_b->sue_pwm_input_4 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_4(msg); - serial_udb_extra_f2_b->sue_pwm_input_5 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_5(msg); - serial_udb_extra_f2_b->sue_pwm_input_6 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_6(msg); - serial_udb_extra_f2_b->sue_pwm_input_7 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_7(msg); - serial_udb_extra_f2_b->sue_pwm_input_8 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_8(msg); - serial_udb_extra_f2_b->sue_pwm_input_9 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_9(msg); - serial_udb_extra_f2_b->sue_pwm_input_10 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_10(msg); - serial_udb_extra_f2_b->sue_pwm_output_1 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_1(msg); - serial_udb_extra_f2_b->sue_pwm_output_2 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_2(msg); - serial_udb_extra_f2_b->sue_pwm_output_3 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_3(msg); - serial_udb_extra_f2_b->sue_pwm_output_4 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_4(msg); - serial_udb_extra_f2_b->sue_pwm_output_5 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_5(msg); - serial_udb_extra_f2_b->sue_pwm_output_6 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_6(msg); - serial_udb_extra_f2_b->sue_pwm_output_7 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_7(msg); - serial_udb_extra_f2_b->sue_pwm_output_8 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_8(msg); - serial_udb_extra_f2_b->sue_pwm_output_9 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_9(msg); - serial_udb_extra_f2_b->sue_pwm_output_10 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_10(msg); - serial_udb_extra_f2_b->sue_imu_location_x = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_x(msg); - serial_udb_extra_f2_b->sue_imu_location_y = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_y(msg); - serial_udb_extra_f2_b->sue_imu_location_z = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_z(msg); - serial_udb_extra_f2_b->sue_osc_fails = mavlink_msg_serial_udb_extra_f2_b_get_sue_osc_fails(msg); - serial_udb_extra_f2_b->sue_imu_velocity_x = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_x(msg); - serial_udb_extra_f2_b->sue_imu_velocity_y = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_y(msg); - serial_udb_extra_f2_b->sue_imu_velocity_z = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_z(msg); - serial_udb_extra_f2_b->sue_waypoint_goal_x = mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_x(msg); - serial_udb_extra_f2_b->sue_waypoint_goal_y = mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_y(msg); - serial_udb_extra_f2_b->sue_waypoint_goal_z = mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_z(msg); - serial_udb_extra_f2_b->sue_memory_stack_free = mavlink_msg_serial_udb_extra_f2_b_get_sue_memory_stack_free(msg); -#else - memcpy(serial_udb_extra_f2_b, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h deleted file mode 100644 index edd1e924c..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h +++ /dev/null @@ -1,425 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F4 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4 172 - -typedef struct __mavlink_serial_udb_extra_f4_t -{ - uint8_t sue_ROLL_STABILIZATION_AILERONS; ///< Serial UDB Extra Roll Stabilization with Ailerons Enabled - uint8_t sue_ROLL_STABILIZATION_RUDDER; ///< Serial UDB Extra Roll Stabilization with Rudder Enabled - uint8_t sue_PITCH_STABILIZATION; ///< Serial UDB Extra Pitch Stabilization Enabled - uint8_t sue_YAW_STABILIZATION_RUDDER; ///< Serial UDB Extra Yaw Stabilization using Rudder Enabled - uint8_t sue_YAW_STABILIZATION_AILERON; ///< Serial UDB Extra Yaw Stabilization using Ailerons Enabled - uint8_t sue_AILERON_NAVIGATION; ///< Serial UDB Extra Navigation with Ailerons Enabled - uint8_t sue_RUDDER_NAVIGATION; ///< Serial UDB Extra Navigation with Rudder Enabled - uint8_t sue_ALTITUDEHOLD_STABILIZED; ///< Serial UDB Extra Type of Alitude Hold when in Stabilized Mode - uint8_t sue_ALTITUDEHOLD_WAYPOINT; ///< Serial UDB Extra Type of Alitude Hold when in Waypoint Mode - uint8_t sue_RACING_MODE; ///< Serial UDB Extra Firmware racing mode enabled -} mavlink_serial_udb_extra_f4_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN 10 -#define MAVLINK_MSG_ID_172_LEN 10 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC 191 -#define MAVLINK_MSG_ID_172_CRC 191 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F4 { \ - "SERIAL_UDB_EXTRA_F4", \ - 10, \ - { { "sue_ROLL_STABILIZATION_AILERONS", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_serial_udb_extra_f4_t, sue_ROLL_STABILIZATION_AILERONS) }, \ - { "sue_ROLL_STABILIZATION_RUDDER", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_serial_udb_extra_f4_t, sue_ROLL_STABILIZATION_RUDDER) }, \ - { "sue_PITCH_STABILIZATION", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_serial_udb_extra_f4_t, sue_PITCH_STABILIZATION) }, \ - { "sue_YAW_STABILIZATION_RUDDER", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_serial_udb_extra_f4_t, sue_YAW_STABILIZATION_RUDDER) }, \ - { "sue_YAW_STABILIZATION_AILERON", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_serial_udb_extra_f4_t, sue_YAW_STABILIZATION_AILERON) }, \ - { "sue_AILERON_NAVIGATION", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_serial_udb_extra_f4_t, sue_AILERON_NAVIGATION) }, \ - { "sue_RUDDER_NAVIGATION", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_serial_udb_extra_f4_t, sue_RUDDER_NAVIGATION) }, \ - { "sue_ALTITUDEHOLD_STABILIZED", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_serial_udb_extra_f4_t, sue_ALTITUDEHOLD_STABILIZED) }, \ - { "sue_ALTITUDEHOLD_WAYPOINT", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_serial_udb_extra_f4_t, sue_ALTITUDEHOLD_WAYPOINT) }, \ - { "sue_RACING_MODE", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_serial_udb_extra_f4_t, sue_RACING_MODE) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f4 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_ROLL_STABILIZATION_AILERONS Serial UDB Extra Roll Stabilization with Ailerons Enabled - * @param sue_ROLL_STABILIZATION_RUDDER Serial UDB Extra Roll Stabilization with Rudder Enabled - * @param sue_PITCH_STABILIZATION Serial UDB Extra Pitch Stabilization Enabled - * @param sue_YAW_STABILIZATION_RUDDER Serial UDB Extra Yaw Stabilization using Rudder Enabled - * @param sue_YAW_STABILIZATION_AILERON Serial UDB Extra Yaw Stabilization using Ailerons Enabled - * @param sue_AILERON_NAVIGATION Serial UDB Extra Navigation with Ailerons Enabled - * @param sue_RUDDER_NAVIGATION Serial UDB Extra Navigation with Rudder Enabled - * @param sue_ALTITUDEHOLD_STABILIZED Serial UDB Extra Type of Alitude Hold when in Stabilized Mode - * @param sue_ALTITUDEHOLD_WAYPOINT Serial UDB Extra Type of Alitude Hold when in Waypoint Mode - * @param sue_RACING_MODE Serial UDB Extra Firmware racing mode enabled - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t sue_ROLL_STABILIZATION_AILERONS, uint8_t sue_ROLL_STABILIZATION_RUDDER, uint8_t sue_PITCH_STABILIZATION, uint8_t sue_YAW_STABILIZATION_RUDDER, uint8_t sue_YAW_STABILIZATION_AILERON, uint8_t sue_AILERON_NAVIGATION, uint8_t sue_RUDDER_NAVIGATION, uint8_t sue_ALTITUDEHOLD_STABILIZED, uint8_t sue_ALTITUDEHOLD_WAYPOINT, uint8_t sue_RACING_MODE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN]; - _mav_put_uint8_t(buf, 0, sue_ROLL_STABILIZATION_AILERONS); - _mav_put_uint8_t(buf, 1, sue_ROLL_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 2, sue_PITCH_STABILIZATION); - _mav_put_uint8_t(buf, 3, sue_YAW_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 4, sue_YAW_STABILIZATION_AILERON); - _mav_put_uint8_t(buf, 5, sue_AILERON_NAVIGATION); - _mav_put_uint8_t(buf, 6, sue_RUDDER_NAVIGATION); - _mav_put_uint8_t(buf, 7, sue_ALTITUDEHOLD_STABILIZED); - _mav_put_uint8_t(buf, 8, sue_ALTITUDEHOLD_WAYPOINT); - _mav_put_uint8_t(buf, 9, sue_RACING_MODE); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#else - mavlink_serial_udb_extra_f4_t packet; - packet.sue_ROLL_STABILIZATION_AILERONS = sue_ROLL_STABILIZATION_AILERONS; - packet.sue_ROLL_STABILIZATION_RUDDER = sue_ROLL_STABILIZATION_RUDDER; - packet.sue_PITCH_STABILIZATION = sue_PITCH_STABILIZATION; - packet.sue_YAW_STABILIZATION_RUDDER = sue_YAW_STABILIZATION_RUDDER; - packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet.sue_AILERON_NAVIGATION = sue_AILERON_NAVIGATION; - packet.sue_RUDDER_NAVIGATION = sue_RUDDER_NAVIGATION; - packet.sue_ALTITUDEHOLD_STABILIZED = sue_ALTITUDEHOLD_STABILIZED; - packet.sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT; - packet.sue_RACING_MODE = sue_RACING_MODE; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f4 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_ROLL_STABILIZATION_AILERONS Serial UDB Extra Roll Stabilization with Ailerons Enabled - * @param sue_ROLL_STABILIZATION_RUDDER Serial UDB Extra Roll Stabilization with Rudder Enabled - * @param sue_PITCH_STABILIZATION Serial UDB Extra Pitch Stabilization Enabled - * @param sue_YAW_STABILIZATION_RUDDER Serial UDB Extra Yaw Stabilization using Rudder Enabled - * @param sue_YAW_STABILIZATION_AILERON Serial UDB Extra Yaw Stabilization using Ailerons Enabled - * @param sue_AILERON_NAVIGATION Serial UDB Extra Navigation with Ailerons Enabled - * @param sue_RUDDER_NAVIGATION Serial UDB Extra Navigation with Rudder Enabled - * @param sue_ALTITUDEHOLD_STABILIZED Serial UDB Extra Type of Alitude Hold when in Stabilized Mode - * @param sue_ALTITUDEHOLD_WAYPOINT Serial UDB Extra Type of Alitude Hold when in Waypoint Mode - * @param sue_RACING_MODE Serial UDB Extra Firmware racing mode enabled - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t sue_ROLL_STABILIZATION_AILERONS,uint8_t sue_ROLL_STABILIZATION_RUDDER,uint8_t sue_PITCH_STABILIZATION,uint8_t sue_YAW_STABILIZATION_RUDDER,uint8_t sue_YAW_STABILIZATION_AILERON,uint8_t sue_AILERON_NAVIGATION,uint8_t sue_RUDDER_NAVIGATION,uint8_t sue_ALTITUDEHOLD_STABILIZED,uint8_t sue_ALTITUDEHOLD_WAYPOINT,uint8_t sue_RACING_MODE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN]; - _mav_put_uint8_t(buf, 0, sue_ROLL_STABILIZATION_AILERONS); - _mav_put_uint8_t(buf, 1, sue_ROLL_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 2, sue_PITCH_STABILIZATION); - _mav_put_uint8_t(buf, 3, sue_YAW_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 4, sue_YAW_STABILIZATION_AILERON); - _mav_put_uint8_t(buf, 5, sue_AILERON_NAVIGATION); - _mav_put_uint8_t(buf, 6, sue_RUDDER_NAVIGATION); - _mav_put_uint8_t(buf, 7, sue_ALTITUDEHOLD_STABILIZED); - _mav_put_uint8_t(buf, 8, sue_ALTITUDEHOLD_WAYPOINT); - _mav_put_uint8_t(buf, 9, sue_RACING_MODE); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#else - mavlink_serial_udb_extra_f4_t packet; - packet.sue_ROLL_STABILIZATION_AILERONS = sue_ROLL_STABILIZATION_AILERONS; - packet.sue_ROLL_STABILIZATION_RUDDER = sue_ROLL_STABILIZATION_RUDDER; - packet.sue_PITCH_STABILIZATION = sue_PITCH_STABILIZATION; - packet.sue_YAW_STABILIZATION_RUDDER = sue_YAW_STABILIZATION_RUDDER; - packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet.sue_AILERON_NAVIGATION = sue_AILERON_NAVIGATION; - packet.sue_RUDDER_NAVIGATION = sue_RUDDER_NAVIGATION; - packet.sue_ALTITUDEHOLD_STABILIZED = sue_ALTITUDEHOLD_STABILIZED; - packet.sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT; - packet.sue_RACING_MODE = sue_RACING_MODE; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f4 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f4 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f4_t* serial_udb_extra_f4) -{ - return mavlink_msg_serial_udb_extra_f4_pack(system_id, component_id, msg, serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS, serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_PITCH_STABILIZATION, serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f4->sue_AILERON_NAVIGATION, serial_udb_extra_f4->sue_RUDDER_NAVIGATION, serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED, serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT, serial_udb_extra_f4->sue_RACING_MODE); -} - -/** - * @brief Encode a serial_udb_extra_f4 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f4 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f4_t* serial_udb_extra_f4) -{ - return mavlink_msg_serial_udb_extra_f4_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS, serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_PITCH_STABILIZATION, serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f4->sue_AILERON_NAVIGATION, serial_udb_extra_f4->sue_RUDDER_NAVIGATION, serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED, serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT, serial_udb_extra_f4->sue_RACING_MODE); -} - -/** - * @brief Send a serial_udb_extra_f4 message - * @param chan MAVLink channel to send the message - * - * @param sue_ROLL_STABILIZATION_AILERONS Serial UDB Extra Roll Stabilization with Ailerons Enabled - * @param sue_ROLL_STABILIZATION_RUDDER Serial UDB Extra Roll Stabilization with Rudder Enabled - * @param sue_PITCH_STABILIZATION Serial UDB Extra Pitch Stabilization Enabled - * @param sue_YAW_STABILIZATION_RUDDER Serial UDB Extra Yaw Stabilization using Rudder Enabled - * @param sue_YAW_STABILIZATION_AILERON Serial UDB Extra Yaw Stabilization using Ailerons Enabled - * @param sue_AILERON_NAVIGATION Serial UDB Extra Navigation with Ailerons Enabled - * @param sue_RUDDER_NAVIGATION Serial UDB Extra Navigation with Rudder Enabled - * @param sue_ALTITUDEHOLD_STABILIZED Serial UDB Extra Type of Alitude Hold when in Stabilized Mode - * @param sue_ALTITUDEHOLD_WAYPOINT Serial UDB Extra Type of Alitude Hold when in Waypoint Mode - * @param sue_RACING_MODE Serial UDB Extra Firmware racing mode enabled - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f4_send(mavlink_channel_t chan, uint8_t sue_ROLL_STABILIZATION_AILERONS, uint8_t sue_ROLL_STABILIZATION_RUDDER, uint8_t sue_PITCH_STABILIZATION, uint8_t sue_YAW_STABILIZATION_RUDDER, uint8_t sue_YAW_STABILIZATION_AILERON, uint8_t sue_AILERON_NAVIGATION, uint8_t sue_RUDDER_NAVIGATION, uint8_t sue_ALTITUDEHOLD_STABILIZED, uint8_t sue_ALTITUDEHOLD_WAYPOINT, uint8_t sue_RACING_MODE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN]; - _mav_put_uint8_t(buf, 0, sue_ROLL_STABILIZATION_AILERONS); - _mav_put_uint8_t(buf, 1, sue_ROLL_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 2, sue_PITCH_STABILIZATION); - _mav_put_uint8_t(buf, 3, sue_YAW_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 4, sue_YAW_STABILIZATION_AILERON); - _mav_put_uint8_t(buf, 5, sue_AILERON_NAVIGATION); - _mav_put_uint8_t(buf, 6, sue_RUDDER_NAVIGATION); - _mav_put_uint8_t(buf, 7, sue_ALTITUDEHOLD_STABILIZED); - _mav_put_uint8_t(buf, 8, sue_ALTITUDEHOLD_WAYPOINT); - _mav_put_uint8_t(buf, 9, sue_RACING_MODE); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif -#else - mavlink_serial_udb_extra_f4_t packet; - packet.sue_ROLL_STABILIZATION_AILERONS = sue_ROLL_STABILIZATION_AILERONS; - packet.sue_ROLL_STABILIZATION_RUDDER = sue_ROLL_STABILIZATION_RUDDER; - packet.sue_PITCH_STABILIZATION = sue_PITCH_STABILIZATION; - packet.sue_YAW_STABILIZATION_RUDDER = sue_YAW_STABILIZATION_RUDDER; - packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet.sue_AILERON_NAVIGATION = sue_AILERON_NAVIGATION; - packet.sue_RUDDER_NAVIGATION = sue_RUDDER_NAVIGATION; - packet.sue_ALTITUDEHOLD_STABILIZED = sue_ALTITUDEHOLD_STABILIZED; - packet.sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT; - packet.sue_RACING_MODE = sue_RACING_MODE; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_serial_udb_extra_f4_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t sue_ROLL_STABILIZATION_AILERONS, uint8_t sue_ROLL_STABILIZATION_RUDDER, uint8_t sue_PITCH_STABILIZATION, uint8_t sue_YAW_STABILIZATION_RUDDER, uint8_t sue_YAW_STABILIZATION_AILERON, uint8_t sue_AILERON_NAVIGATION, uint8_t sue_RUDDER_NAVIGATION, uint8_t sue_ALTITUDEHOLD_STABILIZED, uint8_t sue_ALTITUDEHOLD_WAYPOINT, uint8_t sue_RACING_MODE) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, sue_ROLL_STABILIZATION_AILERONS); - _mav_put_uint8_t(buf, 1, sue_ROLL_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 2, sue_PITCH_STABILIZATION); - _mav_put_uint8_t(buf, 3, sue_YAW_STABILIZATION_RUDDER); - _mav_put_uint8_t(buf, 4, sue_YAW_STABILIZATION_AILERON); - _mav_put_uint8_t(buf, 5, sue_AILERON_NAVIGATION); - _mav_put_uint8_t(buf, 6, sue_RUDDER_NAVIGATION); - _mav_put_uint8_t(buf, 7, sue_ALTITUDEHOLD_STABILIZED); - _mav_put_uint8_t(buf, 8, sue_ALTITUDEHOLD_WAYPOINT); - _mav_put_uint8_t(buf, 9, sue_RACING_MODE); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif -#else - mavlink_serial_udb_extra_f4_t *packet = (mavlink_serial_udb_extra_f4_t *)msgbuf; - packet->sue_ROLL_STABILIZATION_AILERONS = sue_ROLL_STABILIZATION_AILERONS; - packet->sue_ROLL_STABILIZATION_RUDDER = sue_ROLL_STABILIZATION_RUDDER; - packet->sue_PITCH_STABILIZATION = sue_PITCH_STABILIZATION; - packet->sue_YAW_STABILIZATION_RUDDER = sue_YAW_STABILIZATION_RUDDER; - packet->sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet->sue_AILERON_NAVIGATION = sue_AILERON_NAVIGATION; - packet->sue_RUDDER_NAVIGATION = sue_RUDDER_NAVIGATION; - packet->sue_ALTITUDEHOLD_STABILIZED = sue_ALTITUDEHOLD_STABILIZED; - packet->sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT; - packet->sue_RACING_MODE = sue_RACING_MODE; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F4 UNPACKING - - -/** - * @brief Get field sue_ROLL_STABILIZATION_AILERONS from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Roll Stabilization with Ailerons Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_ROLL_STABILIZATION_AILERONS(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field sue_ROLL_STABILIZATION_RUDDER from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Roll Stabilization with Rudder Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_ROLL_STABILIZATION_RUDDER(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field sue_PITCH_STABILIZATION from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Pitch Stabilization Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_PITCH_STABILIZATION(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field sue_YAW_STABILIZATION_RUDDER from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Yaw Stabilization using Rudder Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_YAW_STABILIZATION_RUDDER(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field sue_YAW_STABILIZATION_AILERON from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Yaw Stabilization using Ailerons Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_YAW_STABILIZATION_AILERON(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field sue_AILERON_NAVIGATION from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Navigation with Ailerons Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_AILERON_NAVIGATION(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field sue_RUDDER_NAVIGATION from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Navigation with Rudder Enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_RUDDER_NAVIGATION(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Get field sue_ALTITUDEHOLD_STABILIZED from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Type of Alitude Hold when in Stabilized Mode - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_ALTITUDEHOLD_STABILIZED(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @brief Get field sue_ALTITUDEHOLD_WAYPOINT from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Type of Alitude Hold when in Waypoint Mode - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_ALTITUDEHOLD_WAYPOINT(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field sue_RACING_MODE from serial_udb_extra_f4 message - * - * @return Serial UDB Extra Firmware racing mode enabled - */ -static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_RACING_MODE(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 9); -} - -/** - * @brief Decode a serial_udb_extra_f4 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f4 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f4_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f4_t* serial_udb_extra_f4) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS = mavlink_msg_serial_udb_extra_f4_get_sue_ROLL_STABILIZATION_AILERONS(msg); - serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER = mavlink_msg_serial_udb_extra_f4_get_sue_ROLL_STABILIZATION_RUDDER(msg); - serial_udb_extra_f4->sue_PITCH_STABILIZATION = mavlink_msg_serial_udb_extra_f4_get_sue_PITCH_STABILIZATION(msg); - serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER = mavlink_msg_serial_udb_extra_f4_get_sue_YAW_STABILIZATION_RUDDER(msg); - serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON = mavlink_msg_serial_udb_extra_f4_get_sue_YAW_STABILIZATION_AILERON(msg); - serial_udb_extra_f4->sue_AILERON_NAVIGATION = mavlink_msg_serial_udb_extra_f4_get_sue_AILERON_NAVIGATION(msg); - serial_udb_extra_f4->sue_RUDDER_NAVIGATION = mavlink_msg_serial_udb_extra_f4_get_sue_RUDDER_NAVIGATION(msg); - serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED = mavlink_msg_serial_udb_extra_f4_get_sue_ALTITUDEHOLD_STABILIZED(msg); - serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT = mavlink_msg_serial_udb_extra_f4_get_sue_ALTITUDEHOLD_WAYPOINT(msg); - serial_udb_extra_f4->sue_RACING_MODE = mavlink_msg_serial_udb_extra_f4_get_sue_RACING_MODE(msg); -#else - memcpy(serial_udb_extra_f4, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h deleted file mode 100644 index a821f65bb..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F5 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5 173 - -typedef struct __mavlink_serial_udb_extra_f5_t -{ - float sue_YAWKP_AILERON; ///< Serial UDB YAWKP_AILERON Gain for Proporional control of navigation - float sue_YAWKD_AILERON; ///< Serial UDB YAWKD_AILERON Gain for Rate control of navigation - float sue_ROLLKP; ///< Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization - float sue_ROLLKD; ///< Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization - float sue_YAW_STABILIZATION_AILERON; ///< YAW_STABILIZATION_AILERON Proportional control - float sue_AILERON_BOOST; ///< Gain For Boosting Manual Aileron control When Plane Stabilized -} mavlink_serial_udb_extra_f5_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN 24 -#define MAVLINK_MSG_ID_173_LEN 24 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC 121 -#define MAVLINK_MSG_ID_173_CRC 121 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F5 { \ - "SERIAL_UDB_EXTRA_F5", \ - 6, \ - { { "sue_YAWKP_AILERON", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_serial_udb_extra_f5_t, sue_YAWKP_AILERON) }, \ - { "sue_YAWKD_AILERON", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_serial_udb_extra_f5_t, sue_YAWKD_AILERON) }, \ - { "sue_ROLLKP", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_serial_udb_extra_f5_t, sue_ROLLKP) }, \ - { "sue_ROLLKD", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_serial_udb_extra_f5_t, sue_ROLLKD) }, \ - { "sue_YAW_STABILIZATION_AILERON", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_serial_udb_extra_f5_t, sue_YAW_STABILIZATION_AILERON) }, \ - { "sue_AILERON_BOOST", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_serial_udb_extra_f5_t, sue_AILERON_BOOST) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f5 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_YAWKP_AILERON Serial UDB YAWKP_AILERON Gain for Proporional control of navigation - * @param sue_YAWKD_AILERON Serial UDB YAWKD_AILERON Gain for Rate control of navigation - * @param sue_ROLLKP Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization - * @param sue_ROLLKD Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization - * @param sue_YAW_STABILIZATION_AILERON YAW_STABILIZATION_AILERON Proportional control - * @param sue_AILERON_BOOST Gain For Boosting Manual Aileron control When Plane Stabilized - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float sue_YAWKP_AILERON, float sue_YAWKD_AILERON, float sue_ROLLKP, float sue_ROLLKD, float sue_YAW_STABILIZATION_AILERON, float sue_AILERON_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN]; - _mav_put_float(buf, 0, sue_YAWKP_AILERON); - _mav_put_float(buf, 4, sue_YAWKD_AILERON); - _mav_put_float(buf, 8, sue_ROLLKP); - _mav_put_float(buf, 12, sue_ROLLKD); - _mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON); - _mav_put_float(buf, 20, sue_AILERON_BOOST); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#else - mavlink_serial_udb_extra_f5_t packet; - packet.sue_YAWKP_AILERON = sue_YAWKP_AILERON; - packet.sue_YAWKD_AILERON = sue_YAWKD_AILERON; - packet.sue_ROLLKP = sue_ROLLKP; - packet.sue_ROLLKD = sue_ROLLKD; - packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet.sue_AILERON_BOOST = sue_AILERON_BOOST; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f5 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_YAWKP_AILERON Serial UDB YAWKP_AILERON Gain for Proporional control of navigation - * @param sue_YAWKD_AILERON Serial UDB YAWKD_AILERON Gain for Rate control of navigation - * @param sue_ROLLKP Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization - * @param sue_ROLLKD Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization - * @param sue_YAW_STABILIZATION_AILERON YAW_STABILIZATION_AILERON Proportional control - * @param sue_AILERON_BOOST Gain For Boosting Manual Aileron control When Plane Stabilized - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float sue_YAWKP_AILERON,float sue_YAWKD_AILERON,float sue_ROLLKP,float sue_ROLLKD,float sue_YAW_STABILIZATION_AILERON,float sue_AILERON_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN]; - _mav_put_float(buf, 0, sue_YAWKP_AILERON); - _mav_put_float(buf, 4, sue_YAWKD_AILERON); - _mav_put_float(buf, 8, sue_ROLLKP); - _mav_put_float(buf, 12, sue_ROLLKD); - _mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON); - _mav_put_float(buf, 20, sue_AILERON_BOOST); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#else - mavlink_serial_udb_extra_f5_t packet; - packet.sue_YAWKP_AILERON = sue_YAWKP_AILERON; - packet.sue_YAWKD_AILERON = sue_YAWKD_AILERON; - packet.sue_ROLLKP = sue_ROLLKP; - packet.sue_ROLLKD = sue_ROLLKD; - packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet.sue_AILERON_BOOST = sue_AILERON_BOOST; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f5 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f5 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f5_t* serial_udb_extra_f5) -{ - return mavlink_msg_serial_udb_extra_f5_pack(system_id, component_id, msg, serial_udb_extra_f5->sue_YAWKP_AILERON, serial_udb_extra_f5->sue_YAWKD_AILERON, serial_udb_extra_f5->sue_ROLLKP, serial_udb_extra_f5->sue_ROLLKD, serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f5->sue_AILERON_BOOST); -} - -/** - * @brief Encode a serial_udb_extra_f5 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f5 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f5_t* serial_udb_extra_f5) -{ - return mavlink_msg_serial_udb_extra_f5_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f5->sue_YAWKP_AILERON, serial_udb_extra_f5->sue_YAWKD_AILERON, serial_udb_extra_f5->sue_ROLLKP, serial_udb_extra_f5->sue_ROLLKD, serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f5->sue_AILERON_BOOST); -} - -/** - * @brief Send a serial_udb_extra_f5 message - * @param chan MAVLink channel to send the message - * - * @param sue_YAWKP_AILERON Serial UDB YAWKP_AILERON Gain for Proporional control of navigation - * @param sue_YAWKD_AILERON Serial UDB YAWKD_AILERON Gain for Rate control of navigation - * @param sue_ROLLKP Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization - * @param sue_ROLLKD Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization - * @param sue_YAW_STABILIZATION_AILERON YAW_STABILIZATION_AILERON Proportional control - * @param sue_AILERON_BOOST Gain For Boosting Manual Aileron control When Plane Stabilized - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f5_send(mavlink_channel_t chan, float sue_YAWKP_AILERON, float sue_YAWKD_AILERON, float sue_ROLLKP, float sue_ROLLKD, float sue_YAW_STABILIZATION_AILERON, float sue_AILERON_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN]; - _mav_put_float(buf, 0, sue_YAWKP_AILERON); - _mav_put_float(buf, 4, sue_YAWKD_AILERON); - _mav_put_float(buf, 8, sue_ROLLKP); - _mav_put_float(buf, 12, sue_ROLLKD); - _mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON); - _mav_put_float(buf, 20, sue_AILERON_BOOST); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif -#else - mavlink_serial_udb_extra_f5_t packet; - packet.sue_YAWKP_AILERON = sue_YAWKP_AILERON; - packet.sue_YAWKD_AILERON = sue_YAWKD_AILERON; - packet.sue_ROLLKP = sue_ROLLKP; - packet.sue_ROLLKD = sue_ROLLKD; - packet.sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet.sue_AILERON_BOOST = sue_AILERON_BOOST; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_serial_udb_extra_f5_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float sue_YAWKP_AILERON, float sue_YAWKD_AILERON, float sue_ROLLKP, float sue_ROLLKD, float sue_YAW_STABILIZATION_AILERON, float sue_AILERON_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, sue_YAWKP_AILERON); - _mav_put_float(buf, 4, sue_YAWKD_AILERON); - _mav_put_float(buf, 8, sue_ROLLKP); - _mav_put_float(buf, 12, sue_ROLLKD); - _mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON); - _mav_put_float(buf, 20, sue_AILERON_BOOST); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif -#else - mavlink_serial_udb_extra_f5_t *packet = (mavlink_serial_udb_extra_f5_t *)msgbuf; - packet->sue_YAWKP_AILERON = sue_YAWKP_AILERON; - packet->sue_YAWKD_AILERON = sue_YAWKD_AILERON; - packet->sue_ROLLKP = sue_ROLLKP; - packet->sue_ROLLKD = sue_ROLLKD; - packet->sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; - packet->sue_AILERON_BOOST = sue_AILERON_BOOST; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F5 UNPACKING - - -/** - * @brief Get field sue_YAWKP_AILERON from serial_udb_extra_f5 message - * - * @return Serial UDB YAWKP_AILERON Gain for Proporional control of navigation - */ -static inline float mavlink_msg_serial_udb_extra_f5_get_sue_YAWKP_AILERON(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field sue_YAWKD_AILERON from serial_udb_extra_f5 message - * - * @return Serial UDB YAWKD_AILERON Gain for Rate control of navigation - */ -static inline float mavlink_msg_serial_udb_extra_f5_get_sue_YAWKD_AILERON(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field sue_ROLLKP from serial_udb_extra_f5 message - * - * @return Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization - */ -static inline float mavlink_msg_serial_udb_extra_f5_get_sue_ROLLKP(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field sue_ROLLKD from serial_udb_extra_f5 message - * - * @return Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization - */ -static inline float mavlink_msg_serial_udb_extra_f5_get_sue_ROLLKD(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field sue_YAW_STABILIZATION_AILERON from serial_udb_extra_f5 message - * - * @return YAW_STABILIZATION_AILERON Proportional control - */ -static inline float mavlink_msg_serial_udb_extra_f5_get_sue_YAW_STABILIZATION_AILERON(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field sue_AILERON_BOOST from serial_udb_extra_f5 message - * - * @return Gain For Boosting Manual Aileron control When Plane Stabilized - */ -static inline float mavlink_msg_serial_udb_extra_f5_get_sue_AILERON_BOOST(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a serial_udb_extra_f5 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f5 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f5_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f5_t* serial_udb_extra_f5) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f5->sue_YAWKP_AILERON = mavlink_msg_serial_udb_extra_f5_get_sue_YAWKP_AILERON(msg); - serial_udb_extra_f5->sue_YAWKD_AILERON = mavlink_msg_serial_udb_extra_f5_get_sue_YAWKD_AILERON(msg); - serial_udb_extra_f5->sue_ROLLKP = mavlink_msg_serial_udb_extra_f5_get_sue_ROLLKP(msg); - serial_udb_extra_f5->sue_ROLLKD = mavlink_msg_serial_udb_extra_f5_get_sue_ROLLKD(msg); - serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON = mavlink_msg_serial_udb_extra_f5_get_sue_YAW_STABILIZATION_AILERON(msg); - serial_udb_extra_f5->sue_AILERON_BOOST = mavlink_msg_serial_udb_extra_f5_get_sue_AILERON_BOOST(msg); -#else - memcpy(serial_udb_extra_f5, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h deleted file mode 100644 index 0eb24c986..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F6 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6 174 - -typedef struct __mavlink_serial_udb_extra_f6_t -{ - float sue_PITCHGAIN; ///< Serial UDB Extra PITCHGAIN Proportional Control - float sue_PITCHKD; ///< Serial UDB Extra Pitch Rate Control - float sue_RUDDER_ELEV_MIX; ///< Serial UDB Extra Rudder to Elevator Mix - float sue_ROLL_ELEV_MIX; ///< Serial UDB Extra Roll to Elevator Mix - float sue_ELEVATOR_BOOST; ///< Gain For Boosting Manual Elevator control When Plane Stabilized -} mavlink_serial_udb_extra_f6_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN 20 -#define MAVLINK_MSG_ID_174_LEN 20 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC 54 -#define MAVLINK_MSG_ID_174_CRC 54 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F6 { \ - "SERIAL_UDB_EXTRA_F6", \ - 5, \ - { { "sue_PITCHGAIN", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_serial_udb_extra_f6_t, sue_PITCHGAIN) }, \ - { "sue_PITCHKD", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_serial_udb_extra_f6_t, sue_PITCHKD) }, \ - { "sue_RUDDER_ELEV_MIX", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_serial_udb_extra_f6_t, sue_RUDDER_ELEV_MIX) }, \ - { "sue_ROLL_ELEV_MIX", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_serial_udb_extra_f6_t, sue_ROLL_ELEV_MIX) }, \ - { "sue_ELEVATOR_BOOST", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_serial_udb_extra_f6_t, sue_ELEVATOR_BOOST) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f6 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_PITCHGAIN Serial UDB Extra PITCHGAIN Proportional Control - * @param sue_PITCHKD Serial UDB Extra Pitch Rate Control - * @param sue_RUDDER_ELEV_MIX Serial UDB Extra Rudder to Elevator Mix - * @param sue_ROLL_ELEV_MIX Serial UDB Extra Roll to Elevator Mix - * @param sue_ELEVATOR_BOOST Gain For Boosting Manual Elevator control When Plane Stabilized - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float sue_PITCHGAIN, float sue_PITCHKD, float sue_RUDDER_ELEV_MIX, float sue_ROLL_ELEV_MIX, float sue_ELEVATOR_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN]; - _mav_put_float(buf, 0, sue_PITCHGAIN); - _mav_put_float(buf, 4, sue_PITCHKD); - _mav_put_float(buf, 8, sue_RUDDER_ELEV_MIX); - _mav_put_float(buf, 12, sue_ROLL_ELEV_MIX); - _mav_put_float(buf, 16, sue_ELEVATOR_BOOST); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#else - mavlink_serial_udb_extra_f6_t packet; - packet.sue_PITCHGAIN = sue_PITCHGAIN; - packet.sue_PITCHKD = sue_PITCHKD; - packet.sue_RUDDER_ELEV_MIX = sue_RUDDER_ELEV_MIX; - packet.sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX; - packet.sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f6 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_PITCHGAIN Serial UDB Extra PITCHGAIN Proportional Control - * @param sue_PITCHKD Serial UDB Extra Pitch Rate Control - * @param sue_RUDDER_ELEV_MIX Serial UDB Extra Rudder to Elevator Mix - * @param sue_ROLL_ELEV_MIX Serial UDB Extra Roll to Elevator Mix - * @param sue_ELEVATOR_BOOST Gain For Boosting Manual Elevator control When Plane Stabilized - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float sue_PITCHGAIN,float sue_PITCHKD,float sue_RUDDER_ELEV_MIX,float sue_ROLL_ELEV_MIX,float sue_ELEVATOR_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN]; - _mav_put_float(buf, 0, sue_PITCHGAIN); - _mav_put_float(buf, 4, sue_PITCHKD); - _mav_put_float(buf, 8, sue_RUDDER_ELEV_MIX); - _mav_put_float(buf, 12, sue_ROLL_ELEV_MIX); - _mav_put_float(buf, 16, sue_ELEVATOR_BOOST); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#else - mavlink_serial_udb_extra_f6_t packet; - packet.sue_PITCHGAIN = sue_PITCHGAIN; - packet.sue_PITCHKD = sue_PITCHKD; - packet.sue_RUDDER_ELEV_MIX = sue_RUDDER_ELEV_MIX; - packet.sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX; - packet.sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f6 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f6 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f6_t* serial_udb_extra_f6) -{ - return mavlink_msg_serial_udb_extra_f6_pack(system_id, component_id, msg, serial_udb_extra_f6->sue_PITCHGAIN, serial_udb_extra_f6->sue_PITCHKD, serial_udb_extra_f6->sue_RUDDER_ELEV_MIX, serial_udb_extra_f6->sue_ROLL_ELEV_MIX, serial_udb_extra_f6->sue_ELEVATOR_BOOST); -} - -/** - * @brief Encode a serial_udb_extra_f6 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f6 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f6_t* serial_udb_extra_f6) -{ - return mavlink_msg_serial_udb_extra_f6_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f6->sue_PITCHGAIN, serial_udb_extra_f6->sue_PITCHKD, serial_udb_extra_f6->sue_RUDDER_ELEV_MIX, serial_udb_extra_f6->sue_ROLL_ELEV_MIX, serial_udb_extra_f6->sue_ELEVATOR_BOOST); -} - -/** - * @brief Send a serial_udb_extra_f6 message - * @param chan MAVLink channel to send the message - * - * @param sue_PITCHGAIN Serial UDB Extra PITCHGAIN Proportional Control - * @param sue_PITCHKD Serial UDB Extra Pitch Rate Control - * @param sue_RUDDER_ELEV_MIX Serial UDB Extra Rudder to Elevator Mix - * @param sue_ROLL_ELEV_MIX Serial UDB Extra Roll to Elevator Mix - * @param sue_ELEVATOR_BOOST Gain For Boosting Manual Elevator control When Plane Stabilized - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f6_send(mavlink_channel_t chan, float sue_PITCHGAIN, float sue_PITCHKD, float sue_RUDDER_ELEV_MIX, float sue_ROLL_ELEV_MIX, float sue_ELEVATOR_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN]; - _mav_put_float(buf, 0, sue_PITCHGAIN); - _mav_put_float(buf, 4, sue_PITCHKD); - _mav_put_float(buf, 8, sue_RUDDER_ELEV_MIX); - _mav_put_float(buf, 12, sue_ROLL_ELEV_MIX); - _mav_put_float(buf, 16, sue_ELEVATOR_BOOST); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif -#else - mavlink_serial_udb_extra_f6_t packet; - packet.sue_PITCHGAIN = sue_PITCHGAIN; - packet.sue_PITCHKD = sue_PITCHKD; - packet.sue_RUDDER_ELEV_MIX = sue_RUDDER_ELEV_MIX; - packet.sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX; - packet.sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_serial_udb_extra_f6_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float sue_PITCHGAIN, float sue_PITCHKD, float sue_RUDDER_ELEV_MIX, float sue_ROLL_ELEV_MIX, float sue_ELEVATOR_BOOST) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, sue_PITCHGAIN); - _mav_put_float(buf, 4, sue_PITCHKD); - _mav_put_float(buf, 8, sue_RUDDER_ELEV_MIX); - _mav_put_float(buf, 12, sue_ROLL_ELEV_MIX); - _mav_put_float(buf, 16, sue_ELEVATOR_BOOST); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif -#else - mavlink_serial_udb_extra_f6_t *packet = (mavlink_serial_udb_extra_f6_t *)msgbuf; - packet->sue_PITCHGAIN = sue_PITCHGAIN; - packet->sue_PITCHKD = sue_PITCHKD; - packet->sue_RUDDER_ELEV_MIX = sue_RUDDER_ELEV_MIX; - packet->sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX; - packet->sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F6 UNPACKING - - -/** - * @brief Get field sue_PITCHGAIN from serial_udb_extra_f6 message - * - * @return Serial UDB Extra PITCHGAIN Proportional Control - */ -static inline float mavlink_msg_serial_udb_extra_f6_get_sue_PITCHGAIN(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field sue_PITCHKD from serial_udb_extra_f6 message - * - * @return Serial UDB Extra Pitch Rate Control - */ -static inline float mavlink_msg_serial_udb_extra_f6_get_sue_PITCHKD(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field sue_RUDDER_ELEV_MIX from serial_udb_extra_f6 message - * - * @return Serial UDB Extra Rudder to Elevator Mix - */ -static inline float mavlink_msg_serial_udb_extra_f6_get_sue_RUDDER_ELEV_MIX(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field sue_ROLL_ELEV_MIX from serial_udb_extra_f6 message - * - * @return Serial UDB Extra Roll to Elevator Mix - */ -static inline float mavlink_msg_serial_udb_extra_f6_get_sue_ROLL_ELEV_MIX(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field sue_ELEVATOR_BOOST from serial_udb_extra_f6 message - * - * @return Gain For Boosting Manual Elevator control When Plane Stabilized - */ -static inline float mavlink_msg_serial_udb_extra_f6_get_sue_ELEVATOR_BOOST(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Decode a serial_udb_extra_f6 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f6 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f6_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f6_t* serial_udb_extra_f6) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f6->sue_PITCHGAIN = mavlink_msg_serial_udb_extra_f6_get_sue_PITCHGAIN(msg); - serial_udb_extra_f6->sue_PITCHKD = mavlink_msg_serial_udb_extra_f6_get_sue_PITCHKD(msg); - serial_udb_extra_f6->sue_RUDDER_ELEV_MIX = mavlink_msg_serial_udb_extra_f6_get_sue_RUDDER_ELEV_MIX(msg); - serial_udb_extra_f6->sue_ROLL_ELEV_MIX = mavlink_msg_serial_udb_extra_f6_get_sue_ROLL_ELEV_MIX(msg); - serial_udb_extra_f6->sue_ELEVATOR_BOOST = mavlink_msg_serial_udb_extra_f6_get_sue_ELEVATOR_BOOST(msg); -#else - memcpy(serial_udb_extra_f6, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h deleted file mode 100644 index 58de7d01c..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F7 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7 175 - -typedef struct __mavlink_serial_udb_extra_f7_t -{ - float sue_YAWKP_RUDDER; ///< Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation - float sue_YAWKD_RUDDER; ///< Serial UDB YAWKD_RUDDER Gain for Rate control of navigation - float sue_ROLLKP_RUDDER; ///< Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization - float sue_ROLLKD_RUDDER; ///< Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization - float sue_RUDDER_BOOST; ///< SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized - float sue_RTL_PITCH_DOWN; ///< Serial UDB Extra Return To Landing - Angle to Pitch Plane Down -} mavlink_serial_udb_extra_f7_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN 24 -#define MAVLINK_MSG_ID_175_LEN 24 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC 171 -#define MAVLINK_MSG_ID_175_CRC 171 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F7 { \ - "SERIAL_UDB_EXTRA_F7", \ - 6, \ - { { "sue_YAWKP_RUDDER", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_serial_udb_extra_f7_t, sue_YAWKP_RUDDER) }, \ - { "sue_YAWKD_RUDDER", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_serial_udb_extra_f7_t, sue_YAWKD_RUDDER) }, \ - { "sue_ROLLKP_RUDDER", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_serial_udb_extra_f7_t, sue_ROLLKP_RUDDER) }, \ - { "sue_ROLLKD_RUDDER", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_serial_udb_extra_f7_t, sue_ROLLKD_RUDDER) }, \ - { "sue_RUDDER_BOOST", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_serial_udb_extra_f7_t, sue_RUDDER_BOOST) }, \ - { "sue_RTL_PITCH_DOWN", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_serial_udb_extra_f7_t, sue_RTL_PITCH_DOWN) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f7 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_YAWKP_RUDDER Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation - * @param sue_YAWKD_RUDDER Serial UDB YAWKD_RUDDER Gain for Rate control of navigation - * @param sue_ROLLKP_RUDDER Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization - * @param sue_ROLLKD_RUDDER Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization - * @param sue_RUDDER_BOOST SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized - * @param sue_RTL_PITCH_DOWN Serial UDB Extra Return To Landing - Angle to Pitch Plane Down - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float sue_YAWKP_RUDDER, float sue_YAWKD_RUDDER, float sue_ROLLKP_RUDDER, float sue_ROLLKD_RUDDER, float sue_RUDDER_BOOST, float sue_RTL_PITCH_DOWN) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN]; - _mav_put_float(buf, 0, sue_YAWKP_RUDDER); - _mav_put_float(buf, 4, sue_YAWKD_RUDDER); - _mav_put_float(buf, 8, sue_ROLLKP_RUDDER); - _mav_put_float(buf, 12, sue_ROLLKD_RUDDER); - _mav_put_float(buf, 16, sue_RUDDER_BOOST); - _mav_put_float(buf, 20, sue_RTL_PITCH_DOWN); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#else - mavlink_serial_udb_extra_f7_t packet; - packet.sue_YAWKP_RUDDER = sue_YAWKP_RUDDER; - packet.sue_YAWKD_RUDDER = sue_YAWKD_RUDDER; - packet.sue_ROLLKP_RUDDER = sue_ROLLKP_RUDDER; - packet.sue_ROLLKD_RUDDER = sue_ROLLKD_RUDDER; - packet.sue_RUDDER_BOOST = sue_RUDDER_BOOST; - packet.sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f7 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_YAWKP_RUDDER Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation - * @param sue_YAWKD_RUDDER Serial UDB YAWKD_RUDDER Gain for Rate control of navigation - * @param sue_ROLLKP_RUDDER Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization - * @param sue_ROLLKD_RUDDER Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization - * @param sue_RUDDER_BOOST SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized - * @param sue_RTL_PITCH_DOWN Serial UDB Extra Return To Landing - Angle to Pitch Plane Down - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float sue_YAWKP_RUDDER,float sue_YAWKD_RUDDER,float sue_ROLLKP_RUDDER,float sue_ROLLKD_RUDDER,float sue_RUDDER_BOOST,float sue_RTL_PITCH_DOWN) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN]; - _mav_put_float(buf, 0, sue_YAWKP_RUDDER); - _mav_put_float(buf, 4, sue_YAWKD_RUDDER); - _mav_put_float(buf, 8, sue_ROLLKP_RUDDER); - _mav_put_float(buf, 12, sue_ROLLKD_RUDDER); - _mav_put_float(buf, 16, sue_RUDDER_BOOST); - _mav_put_float(buf, 20, sue_RTL_PITCH_DOWN); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#else - mavlink_serial_udb_extra_f7_t packet; - packet.sue_YAWKP_RUDDER = sue_YAWKP_RUDDER; - packet.sue_YAWKD_RUDDER = sue_YAWKD_RUDDER; - packet.sue_ROLLKP_RUDDER = sue_ROLLKP_RUDDER; - packet.sue_ROLLKD_RUDDER = sue_ROLLKD_RUDDER; - packet.sue_RUDDER_BOOST = sue_RUDDER_BOOST; - packet.sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f7 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f7 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f7_t* serial_udb_extra_f7) -{ - return mavlink_msg_serial_udb_extra_f7_pack(system_id, component_id, msg, serial_udb_extra_f7->sue_YAWKP_RUDDER, serial_udb_extra_f7->sue_YAWKD_RUDDER, serial_udb_extra_f7->sue_ROLLKP_RUDDER, serial_udb_extra_f7->sue_ROLLKD_RUDDER, serial_udb_extra_f7->sue_RUDDER_BOOST, serial_udb_extra_f7->sue_RTL_PITCH_DOWN); -} - -/** - * @brief Encode a serial_udb_extra_f7 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f7 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f7_t* serial_udb_extra_f7) -{ - return mavlink_msg_serial_udb_extra_f7_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f7->sue_YAWKP_RUDDER, serial_udb_extra_f7->sue_YAWKD_RUDDER, serial_udb_extra_f7->sue_ROLLKP_RUDDER, serial_udb_extra_f7->sue_ROLLKD_RUDDER, serial_udb_extra_f7->sue_RUDDER_BOOST, serial_udb_extra_f7->sue_RTL_PITCH_DOWN); -} - -/** - * @brief Send a serial_udb_extra_f7 message - * @param chan MAVLink channel to send the message - * - * @param sue_YAWKP_RUDDER Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation - * @param sue_YAWKD_RUDDER Serial UDB YAWKD_RUDDER Gain for Rate control of navigation - * @param sue_ROLLKP_RUDDER Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization - * @param sue_ROLLKD_RUDDER Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization - * @param sue_RUDDER_BOOST SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized - * @param sue_RTL_PITCH_DOWN Serial UDB Extra Return To Landing - Angle to Pitch Plane Down - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f7_send(mavlink_channel_t chan, float sue_YAWKP_RUDDER, float sue_YAWKD_RUDDER, float sue_ROLLKP_RUDDER, float sue_ROLLKD_RUDDER, float sue_RUDDER_BOOST, float sue_RTL_PITCH_DOWN) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN]; - _mav_put_float(buf, 0, sue_YAWKP_RUDDER); - _mav_put_float(buf, 4, sue_YAWKD_RUDDER); - _mav_put_float(buf, 8, sue_ROLLKP_RUDDER); - _mav_put_float(buf, 12, sue_ROLLKD_RUDDER); - _mav_put_float(buf, 16, sue_RUDDER_BOOST); - _mav_put_float(buf, 20, sue_RTL_PITCH_DOWN); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif -#else - mavlink_serial_udb_extra_f7_t packet; - packet.sue_YAWKP_RUDDER = sue_YAWKP_RUDDER; - packet.sue_YAWKD_RUDDER = sue_YAWKD_RUDDER; - packet.sue_ROLLKP_RUDDER = sue_ROLLKP_RUDDER; - packet.sue_ROLLKD_RUDDER = sue_ROLLKD_RUDDER; - packet.sue_RUDDER_BOOST = sue_RUDDER_BOOST; - packet.sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_serial_udb_extra_f7_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float sue_YAWKP_RUDDER, float sue_YAWKD_RUDDER, float sue_ROLLKP_RUDDER, float sue_ROLLKD_RUDDER, float sue_RUDDER_BOOST, float sue_RTL_PITCH_DOWN) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, sue_YAWKP_RUDDER); - _mav_put_float(buf, 4, sue_YAWKD_RUDDER); - _mav_put_float(buf, 8, sue_ROLLKP_RUDDER); - _mav_put_float(buf, 12, sue_ROLLKD_RUDDER); - _mav_put_float(buf, 16, sue_RUDDER_BOOST); - _mav_put_float(buf, 20, sue_RTL_PITCH_DOWN); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif -#else - mavlink_serial_udb_extra_f7_t *packet = (mavlink_serial_udb_extra_f7_t *)msgbuf; - packet->sue_YAWKP_RUDDER = sue_YAWKP_RUDDER; - packet->sue_YAWKD_RUDDER = sue_YAWKD_RUDDER; - packet->sue_ROLLKP_RUDDER = sue_ROLLKP_RUDDER; - packet->sue_ROLLKD_RUDDER = sue_ROLLKD_RUDDER; - packet->sue_RUDDER_BOOST = sue_RUDDER_BOOST; - packet->sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F7 UNPACKING - - -/** - * @brief Get field sue_YAWKP_RUDDER from serial_udb_extra_f7 message - * - * @return Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation - */ -static inline float mavlink_msg_serial_udb_extra_f7_get_sue_YAWKP_RUDDER(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field sue_YAWKD_RUDDER from serial_udb_extra_f7 message - * - * @return Serial UDB YAWKD_RUDDER Gain for Rate control of navigation - */ -static inline float mavlink_msg_serial_udb_extra_f7_get_sue_YAWKD_RUDDER(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field sue_ROLLKP_RUDDER from serial_udb_extra_f7 message - * - * @return Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization - */ -static inline float mavlink_msg_serial_udb_extra_f7_get_sue_ROLLKP_RUDDER(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field sue_ROLLKD_RUDDER from serial_udb_extra_f7 message - * - * @return Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization - */ -static inline float mavlink_msg_serial_udb_extra_f7_get_sue_ROLLKD_RUDDER(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field sue_RUDDER_BOOST from serial_udb_extra_f7 message - * - * @return SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized - */ -static inline float mavlink_msg_serial_udb_extra_f7_get_sue_RUDDER_BOOST(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field sue_RTL_PITCH_DOWN from serial_udb_extra_f7 message - * - * @return Serial UDB Extra Return To Landing - Angle to Pitch Plane Down - */ -static inline float mavlink_msg_serial_udb_extra_f7_get_sue_RTL_PITCH_DOWN(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a serial_udb_extra_f7 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f7 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f7_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f7_t* serial_udb_extra_f7) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f7->sue_YAWKP_RUDDER = mavlink_msg_serial_udb_extra_f7_get_sue_YAWKP_RUDDER(msg); - serial_udb_extra_f7->sue_YAWKD_RUDDER = mavlink_msg_serial_udb_extra_f7_get_sue_YAWKD_RUDDER(msg); - serial_udb_extra_f7->sue_ROLLKP_RUDDER = mavlink_msg_serial_udb_extra_f7_get_sue_ROLLKP_RUDDER(msg); - serial_udb_extra_f7->sue_ROLLKD_RUDDER = mavlink_msg_serial_udb_extra_f7_get_sue_ROLLKD_RUDDER(msg); - serial_udb_extra_f7->sue_RUDDER_BOOST = mavlink_msg_serial_udb_extra_f7_get_sue_RUDDER_BOOST(msg); - serial_udb_extra_f7->sue_RTL_PITCH_DOWN = mavlink_msg_serial_udb_extra_f7_get_sue_RTL_PITCH_DOWN(msg); -#else - memcpy(serial_udb_extra_f7, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h deleted file mode 100644 index 5cab81655..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE SERIAL_UDB_EXTRA_F8 PACKING - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8 176 - -typedef struct __mavlink_serial_udb_extra_f8_t -{ - float sue_HEIGHT_TARGET_MAX; ///< Serial UDB Extra HEIGHT_TARGET_MAX - float sue_HEIGHT_TARGET_MIN; ///< Serial UDB Extra HEIGHT_TARGET_MIN - float sue_ALT_HOLD_THROTTLE_MIN; ///< Serial UDB Extra ALT_HOLD_THROTTLE_MIN - float sue_ALT_HOLD_THROTTLE_MAX; ///< Serial UDB Extra ALT_HOLD_THROTTLE_MAX - float sue_ALT_HOLD_PITCH_MIN; ///< Serial UDB Extra ALT_HOLD_PITCH_MIN - float sue_ALT_HOLD_PITCH_MAX; ///< Serial UDB Extra ALT_HOLD_PITCH_MAX - float sue_ALT_HOLD_PITCH_HIGH; ///< Serial UDB Extra ALT_HOLD_PITCH_HIGH -} mavlink_serial_udb_extra_f8_t; - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN 28 -#define MAVLINK_MSG_ID_176_LEN 28 - -#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC 142 -#define MAVLINK_MSG_ID_176_CRC 142 - - - -#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F8 { \ - "SERIAL_UDB_EXTRA_F8", \ - 7, \ - { { "sue_HEIGHT_TARGET_MAX", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_serial_udb_extra_f8_t, sue_HEIGHT_TARGET_MAX) }, \ - { "sue_HEIGHT_TARGET_MIN", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_serial_udb_extra_f8_t, sue_HEIGHT_TARGET_MIN) }, \ - { "sue_ALT_HOLD_THROTTLE_MIN", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_THROTTLE_MIN) }, \ - { "sue_ALT_HOLD_THROTTLE_MAX", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_THROTTLE_MAX) }, \ - { "sue_ALT_HOLD_PITCH_MIN", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_PITCH_MIN) }, \ - { "sue_ALT_HOLD_PITCH_MAX", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_PITCH_MAX) }, \ - { "sue_ALT_HOLD_PITCH_HIGH", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_PITCH_HIGH) }, \ - } \ -} - - -/** - * @brief Pack a serial_udb_extra_f8 message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param sue_HEIGHT_TARGET_MAX Serial UDB Extra HEIGHT_TARGET_MAX - * @param sue_HEIGHT_TARGET_MIN Serial UDB Extra HEIGHT_TARGET_MIN - * @param sue_ALT_HOLD_THROTTLE_MIN Serial UDB Extra ALT_HOLD_THROTTLE_MIN - * @param sue_ALT_HOLD_THROTTLE_MAX Serial UDB Extra ALT_HOLD_THROTTLE_MAX - * @param sue_ALT_HOLD_PITCH_MIN Serial UDB Extra ALT_HOLD_PITCH_MIN - * @param sue_ALT_HOLD_PITCH_MAX Serial UDB Extra ALT_HOLD_PITCH_MAX - * @param sue_ALT_HOLD_PITCH_HIGH Serial UDB Extra ALT_HOLD_PITCH_HIGH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float sue_HEIGHT_TARGET_MAX, float sue_HEIGHT_TARGET_MIN, float sue_ALT_HOLD_THROTTLE_MIN, float sue_ALT_HOLD_THROTTLE_MAX, float sue_ALT_HOLD_PITCH_MIN, float sue_ALT_HOLD_PITCH_MAX, float sue_ALT_HOLD_PITCH_HIGH) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN]; - _mav_put_float(buf, 0, sue_HEIGHT_TARGET_MAX); - _mav_put_float(buf, 4, sue_HEIGHT_TARGET_MIN); - _mav_put_float(buf, 8, sue_ALT_HOLD_THROTTLE_MIN); - _mav_put_float(buf, 12, sue_ALT_HOLD_THROTTLE_MAX); - _mav_put_float(buf, 16, sue_ALT_HOLD_PITCH_MIN); - _mav_put_float(buf, 20, sue_ALT_HOLD_PITCH_MAX); - _mav_put_float(buf, 24, sue_ALT_HOLD_PITCH_HIGH); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#else - mavlink_serial_udb_extra_f8_t packet; - packet.sue_HEIGHT_TARGET_MAX = sue_HEIGHT_TARGET_MAX; - packet.sue_HEIGHT_TARGET_MIN = sue_HEIGHT_TARGET_MIN; - packet.sue_ALT_HOLD_THROTTLE_MIN = sue_ALT_HOLD_THROTTLE_MIN; - packet.sue_ALT_HOLD_THROTTLE_MAX = sue_ALT_HOLD_THROTTLE_MAX; - packet.sue_ALT_HOLD_PITCH_MIN = sue_ALT_HOLD_PITCH_MIN; - packet.sue_ALT_HOLD_PITCH_MAX = sue_ALT_HOLD_PITCH_MAX; - packet.sue_ALT_HOLD_PITCH_HIGH = sue_ALT_HOLD_PITCH_HIGH; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif -} - -/** - * @brief Pack a serial_udb_extra_f8 message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sue_HEIGHT_TARGET_MAX Serial UDB Extra HEIGHT_TARGET_MAX - * @param sue_HEIGHT_TARGET_MIN Serial UDB Extra HEIGHT_TARGET_MIN - * @param sue_ALT_HOLD_THROTTLE_MIN Serial UDB Extra ALT_HOLD_THROTTLE_MIN - * @param sue_ALT_HOLD_THROTTLE_MAX Serial UDB Extra ALT_HOLD_THROTTLE_MAX - * @param sue_ALT_HOLD_PITCH_MIN Serial UDB Extra ALT_HOLD_PITCH_MIN - * @param sue_ALT_HOLD_PITCH_MAX Serial UDB Extra ALT_HOLD_PITCH_MAX - * @param sue_ALT_HOLD_PITCH_HIGH Serial UDB Extra ALT_HOLD_PITCH_HIGH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float sue_HEIGHT_TARGET_MAX,float sue_HEIGHT_TARGET_MIN,float sue_ALT_HOLD_THROTTLE_MIN,float sue_ALT_HOLD_THROTTLE_MAX,float sue_ALT_HOLD_PITCH_MIN,float sue_ALT_HOLD_PITCH_MAX,float sue_ALT_HOLD_PITCH_HIGH) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN]; - _mav_put_float(buf, 0, sue_HEIGHT_TARGET_MAX); - _mav_put_float(buf, 4, sue_HEIGHT_TARGET_MIN); - _mav_put_float(buf, 8, sue_ALT_HOLD_THROTTLE_MIN); - _mav_put_float(buf, 12, sue_ALT_HOLD_THROTTLE_MAX); - _mav_put_float(buf, 16, sue_ALT_HOLD_PITCH_MIN); - _mav_put_float(buf, 20, sue_ALT_HOLD_PITCH_MAX); - _mav_put_float(buf, 24, sue_ALT_HOLD_PITCH_HIGH); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#else - mavlink_serial_udb_extra_f8_t packet; - packet.sue_HEIGHT_TARGET_MAX = sue_HEIGHT_TARGET_MAX; - packet.sue_HEIGHT_TARGET_MIN = sue_HEIGHT_TARGET_MIN; - packet.sue_ALT_HOLD_THROTTLE_MIN = sue_ALT_HOLD_THROTTLE_MIN; - packet.sue_ALT_HOLD_THROTTLE_MAX = sue_ALT_HOLD_THROTTLE_MAX; - packet.sue_ALT_HOLD_PITCH_MIN = sue_ALT_HOLD_PITCH_MIN; - packet.sue_ALT_HOLD_PITCH_MAX = sue_ALT_HOLD_PITCH_MAX; - packet.sue_ALT_HOLD_PITCH_HIGH = sue_ALT_HOLD_PITCH_HIGH; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif -} - -/** - * @brief Encode a serial_udb_extra_f8 struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f8 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f8_t* serial_udb_extra_f8) -{ - return mavlink_msg_serial_udb_extra_f8_pack(system_id, component_id, msg, serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX, serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH); -} - -/** - * @brief Encode a serial_udb_extra_f8 struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param serial_udb_extra_f8 C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f8_t* serial_udb_extra_f8) -{ - return mavlink_msg_serial_udb_extra_f8_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX, serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH); -} - -/** - * @brief Send a serial_udb_extra_f8 message - * @param chan MAVLink channel to send the message - * - * @param sue_HEIGHT_TARGET_MAX Serial UDB Extra HEIGHT_TARGET_MAX - * @param sue_HEIGHT_TARGET_MIN Serial UDB Extra HEIGHT_TARGET_MIN - * @param sue_ALT_HOLD_THROTTLE_MIN Serial UDB Extra ALT_HOLD_THROTTLE_MIN - * @param sue_ALT_HOLD_THROTTLE_MAX Serial UDB Extra ALT_HOLD_THROTTLE_MAX - * @param sue_ALT_HOLD_PITCH_MIN Serial UDB Extra ALT_HOLD_PITCH_MIN - * @param sue_ALT_HOLD_PITCH_MAX Serial UDB Extra ALT_HOLD_PITCH_MAX - * @param sue_ALT_HOLD_PITCH_HIGH Serial UDB Extra ALT_HOLD_PITCH_HIGH - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_serial_udb_extra_f8_send(mavlink_channel_t chan, float sue_HEIGHT_TARGET_MAX, float sue_HEIGHT_TARGET_MIN, float sue_ALT_HOLD_THROTTLE_MIN, float sue_ALT_HOLD_THROTTLE_MAX, float sue_ALT_HOLD_PITCH_MIN, float sue_ALT_HOLD_PITCH_MAX, float sue_ALT_HOLD_PITCH_HIGH) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN]; - _mav_put_float(buf, 0, sue_HEIGHT_TARGET_MAX); - _mav_put_float(buf, 4, sue_HEIGHT_TARGET_MIN); - _mav_put_float(buf, 8, sue_ALT_HOLD_THROTTLE_MIN); - _mav_put_float(buf, 12, sue_ALT_HOLD_THROTTLE_MAX); - _mav_put_float(buf, 16, sue_ALT_HOLD_PITCH_MIN); - _mav_put_float(buf, 20, sue_ALT_HOLD_PITCH_MAX); - _mav_put_float(buf, 24, sue_ALT_HOLD_PITCH_HIGH); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif -#else - mavlink_serial_udb_extra_f8_t packet; - packet.sue_HEIGHT_TARGET_MAX = sue_HEIGHT_TARGET_MAX; - packet.sue_HEIGHT_TARGET_MIN = sue_HEIGHT_TARGET_MIN; - packet.sue_ALT_HOLD_THROTTLE_MIN = sue_ALT_HOLD_THROTTLE_MIN; - packet.sue_ALT_HOLD_THROTTLE_MAX = sue_ALT_HOLD_THROTTLE_MAX; - packet.sue_ALT_HOLD_PITCH_MIN = sue_ALT_HOLD_PITCH_MIN; - packet.sue_ALT_HOLD_PITCH_MAX = sue_ALT_HOLD_PITCH_MAX; - packet.sue_ALT_HOLD_PITCH_HIGH = sue_ALT_HOLD_PITCH_HIGH; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_serial_udb_extra_f8_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float sue_HEIGHT_TARGET_MAX, float sue_HEIGHT_TARGET_MIN, float sue_ALT_HOLD_THROTTLE_MIN, float sue_ALT_HOLD_THROTTLE_MAX, float sue_ALT_HOLD_PITCH_MIN, float sue_ALT_HOLD_PITCH_MAX, float sue_ALT_HOLD_PITCH_HIGH) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, sue_HEIGHT_TARGET_MAX); - _mav_put_float(buf, 4, sue_HEIGHT_TARGET_MIN); - _mav_put_float(buf, 8, sue_ALT_HOLD_THROTTLE_MIN); - _mav_put_float(buf, 12, sue_ALT_HOLD_THROTTLE_MAX); - _mav_put_float(buf, 16, sue_ALT_HOLD_PITCH_MIN); - _mav_put_float(buf, 20, sue_ALT_HOLD_PITCH_MAX); - _mav_put_float(buf, 24, sue_ALT_HOLD_PITCH_HIGH); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif -#else - mavlink_serial_udb_extra_f8_t *packet = (mavlink_serial_udb_extra_f8_t *)msgbuf; - packet->sue_HEIGHT_TARGET_MAX = sue_HEIGHT_TARGET_MAX; - packet->sue_HEIGHT_TARGET_MIN = sue_HEIGHT_TARGET_MIN; - packet->sue_ALT_HOLD_THROTTLE_MIN = sue_ALT_HOLD_THROTTLE_MIN; - packet->sue_ALT_HOLD_THROTTLE_MAX = sue_ALT_HOLD_THROTTLE_MAX; - packet->sue_ALT_HOLD_PITCH_MIN = sue_ALT_HOLD_PITCH_MIN; - packet->sue_ALT_HOLD_PITCH_MAX = sue_ALT_HOLD_PITCH_MAX; - packet->sue_ALT_HOLD_PITCH_HIGH = sue_ALT_HOLD_PITCH_HIGH; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SERIAL_UDB_EXTRA_F8 UNPACKING - - -/** - * @brief Get field sue_HEIGHT_TARGET_MAX from serial_udb_extra_f8 message - * - * @return Serial UDB Extra HEIGHT_TARGET_MAX - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_HEIGHT_TARGET_MAX(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field sue_HEIGHT_TARGET_MIN from serial_udb_extra_f8 message - * - * @return Serial UDB Extra HEIGHT_TARGET_MIN - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_HEIGHT_TARGET_MIN(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field sue_ALT_HOLD_THROTTLE_MIN from serial_udb_extra_f8 message - * - * @return Serial UDB Extra ALT_HOLD_THROTTLE_MIN - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_THROTTLE_MIN(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field sue_ALT_HOLD_THROTTLE_MAX from serial_udb_extra_f8 message - * - * @return Serial UDB Extra ALT_HOLD_THROTTLE_MAX - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_THROTTLE_MAX(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field sue_ALT_HOLD_PITCH_MIN from serial_udb_extra_f8 message - * - * @return Serial UDB Extra ALT_HOLD_PITCH_MIN - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_MIN(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field sue_ALT_HOLD_PITCH_MAX from serial_udb_extra_f8 message - * - * @return Serial UDB Extra ALT_HOLD_PITCH_MAX - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_MAX(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field sue_ALT_HOLD_PITCH_HIGH from serial_udb_extra_f8 message - * - * @return Serial UDB Extra ALT_HOLD_PITCH_HIGH - */ -static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_HIGH(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Decode a serial_udb_extra_f8 message into a struct - * - * @param msg The message to decode - * @param serial_udb_extra_f8 C-struct to decode the message contents into - */ -static inline void mavlink_msg_serial_udb_extra_f8_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f8_t* serial_udb_extra_f8) -{ -#if MAVLINK_NEED_BYTE_SWAP - serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX = mavlink_msg_serial_udb_extra_f8_get_sue_HEIGHT_TARGET_MAX(msg); - serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN = mavlink_msg_serial_udb_extra_f8_get_sue_HEIGHT_TARGET_MIN(msg); - serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_THROTTLE_MIN(msg); - serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_THROTTLE_MAX(msg); - serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_MIN(msg); - serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_MAX(msg); - serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_HIGH(msg); -#else - memcpy(serial_udb_extra_f8, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h b/mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h deleted file mode 100644 index 6856839e9..000000000 --- a/mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h +++ /dev/null @@ -1,1240 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from matrixpilot.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef MATRIXPILOT_TESTSUITE_H -#define MATRIXPILOT_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_matrixpilot(uint8_t, uint8_t, mavlink_message_t *last_msg); - -static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_test_common(system_id, component_id, last_msg); - mavlink_test_matrixpilot(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_flexifunction_set(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_flexifunction_set_t packet_in = { - 5, - }72, - }; - mavlink_flexifunction_set_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.target_system = packet_in.target_system; - packet1.target_component = packet_in.target_component; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_flexifunction_set_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_flexifunction_set_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_flexifunction_set_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); - mavlink_msg_flexifunction_set_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_flexifunction_set_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); - mavlink_msg_flexifunction_set_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; i - -#ifndef M_PI_2 - #define M_PI_2 ((float)asin(1)) -#endif - -/** - * @file mavlink_conversions.h - * - * These conversion functions follow the NASA rotation standards definition file - * available online. - * - * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free - * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free) - * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the - * protocol as widely as possible. - * - * @author James Goppert - */ - -MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3]) -{ - double a = quaternion[0]; - double b = quaternion[1]; - double c = quaternion[2]; - double d = quaternion[3]; - double aSq = a * a; - double bSq = b * b; - double cSq = c * c; - double dSq = d * d; - dcm[0][0] = aSq + bSq - cSq - dSq; - dcm[0][1] = 2.0 * (b * c - a * d); - dcm[0][2] = 2.0 * (a * c + b * d); - dcm[1][0] = 2.0 * (b * c + a * d); - dcm[1][1] = aSq - bSq + cSq - dSq; - dcm[1][2] = 2.0 * (c * d - a * b); - dcm[2][0] = 2.0 * (b * d - a * c); - dcm[2][1] = 2.0 * (a * b + c * d); - dcm[2][2] = aSq - bSq - cSq + dSq; -} - -MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw) -{ - float phi, theta, psi; - theta = asin(-dcm[2][0]); - - if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) { - phi = 0.0f; - psi = (atan2f(dcm[1][2] - dcm[0][1], - dcm[0][2] + dcm[1][1]) + phi); - - } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) { - phi = 0.0f; - psi = atan2f(dcm[1][2] - dcm[0][1], - dcm[0][2] + dcm[1][1] - phi); - - } else { - phi = atan2f(dcm[2][1], dcm[2][2]); - psi = atan2f(dcm[1][0], dcm[0][0]); - } - - *roll = phi; - *pitch = theta; - *yaw = psi; -} - -MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw) -{ - float dcm[3][3]; - mavlink_quaternion_to_dcm(quaternion, dcm); - mavlink_dcm_to_euler(dcm, roll, pitch, yaw); -} - -MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4]) -{ - double cosPhi_2 = cos((double)roll / 2.0); - double sinPhi_2 = sin((double)roll / 2.0); - double cosTheta_2 = cos((double)pitch / 2.0); - double sinTheta_2 = sin((double)pitch / 2.0); - double cosPsi_2 = cos((double)yaw / 2.0); - double sinPsi_2 = sin((double)yaw / 2.0); - quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + - sinPhi_2 * sinTheta_2 * sinPsi_2); - quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - - cosPhi_2 * sinTheta_2 * sinPsi_2); - quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + - sinPhi_2 * cosTheta_2 * sinPsi_2); - quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - - sinPhi_2 * sinTheta_2 * cosPsi_2); -} - -MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4]) -{ - quaternion[0] = (0.5 * sqrt(1.0 + - (double)(dcm[0][0] + dcm[1][1] + dcm[2][2]))); - quaternion[1] = (0.5 * sqrt(1.0 + - (double)(dcm[0][0] - dcm[1][1] - dcm[2][2]))); - quaternion[2] = (0.5 * sqrt(1.0 + - (double)(-dcm[0][0] + dcm[1][1] - dcm[2][2]))); - quaternion[3] = (0.5 * sqrt(1.0 + - (double)(-dcm[0][0] - dcm[1][1] + dcm[2][2]))); -} - -MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3]) -{ - double cosPhi = cos(roll); - double sinPhi = sin(roll); - double cosThe = cos(pitch); - double sinThe = sin(pitch); - double cosPsi = cos(yaw); - double sinPsi = sin(yaw); - - dcm[0][0] = cosThe * cosPsi; - dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; - dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; - - dcm[1][0] = cosThe * sinPsi; - dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; - dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; - - dcm[2][0] = -sinThe; - dcm[2][1] = sinPhi * cosThe; - dcm[2][2] = cosPhi * cosThe; -} - -#endif diff --git a/mavlink/include/mavlink/v1.0/mavlink_helpers.h b/mavlink/include/mavlink/v1.0/mavlink_helpers.h deleted file mode 100644 index 1639a830b..000000000 --- a/mavlink/include/mavlink/v1.0/mavlink_helpers.h +++ /dev/null @@ -1,574 +0,0 @@ -#ifndef _MAVLINK_HELPERS_H_ -#define _MAVLINK_HELPERS_H_ - -#include "string.h" -#include "checksum.h" -#include "mavlink_types.h" -#include "mavlink_conversions.h" - -#ifndef MAVLINK_HELPER -#define MAVLINK_HELPER -#endif - -/* - * Internal function to give access to the channel status for each channel - */ -#ifndef MAVLINK_GET_CHANNEL_STATUS -MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) -{ -#if MAVLINK_EXTERNAL_RX_STATUS - // No m_mavlink_status array defined in function, - // has to be defined externally -#else - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; -#endif - return &m_mavlink_status[chan]; -} -#endif - -/* - * Internal function to give access to the channel buffer for each channel - */ -#ifndef MAVLINK_GET_CHANNEL_BUFFER -MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) -{ - -#if MAVLINK_EXTERNAL_RX_BUFFER - // No m_mavlink_buffer array defined in function, - // has to be defined externally -#else - static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; -#endif - return &m_mavlink_buffer[chan]; -} -#endif - -/** - * @brief Reset the status of a channel. - */ -MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan) -{ - mavlink_status_t *status = mavlink_get_channel_status(chan); - status->parse_state = MAVLINK_PARSE_STATE_IDLE; -} - -/** - * @brief Finalize a MAVLink message with channel assignment - * - * This function calculates the checksum and sets length and aircraft id correctly. - * It assumes that the message id and the payload are already correctly set. This function - * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack - * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. - * - * @param msg Message to finalize - * @param system_id Id of the sending (this) system, 1-127 - * @param length Message length - */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length, uint8_t crc_extra) -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length) -#endif -{ - // This code part is the same for all messages; - msg->magic = MAVLINK_STX; - msg->len = length; - msg->sysid = system_id; - msg->compid = component_id; - // One sequence number per component - msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN); - crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len); -#if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &msg->checksum); -#endif - mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF); - mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8); - - return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; -} - - -/** - * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel - */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length, uint8_t crc_extra) -{ - return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra); -} -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length) -{ - return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); - -/** - * @brief Finalize a MAVLink message with channel assignment and send - */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, - uint8_t length, uint8_t crc_extra) -#else -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length) -#endif -{ - uint16_t checksum; - uint8_t buf[MAVLINK_NUM_HEADER_BYTES]; - uint8_t ck[2]; - mavlink_status_t *status = mavlink_get_channel_status(chan); - buf[0] = MAVLINK_STX; - buf[1] = length; - buf[2] = status->current_tx_seq; - buf[3] = mavlink_system.sysid; - buf[4] = mavlink_system.compid; - buf[5] = msgid; - status->current_tx_seq++; - checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); - crc_accumulate_buffer(&checksum, packet, length); -#if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); -#endif - ck[0] = (uint8_t)(checksum & 0xFF); - ck[1] = (uint8_t)(checksum >> 8); - - MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); - _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES); - _mavlink_send_uart(chan, packet, length); - _mavlink_send_uart(chan, (const char *)ck, 2); - MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); -} - -/** - * @brief re-send a message over a uart channel - * this is more stack efficient than re-marshalling the message - */ -MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg) -{ - uint8_t ck[2]; - - ck[0] = (uint8_t)(msg->checksum & 0xFF); - ck[1] = (uint8_t)(msg->checksum >> 8); - // XXX use the right sequence here - - MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); - _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); - _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len); - _mavlink_send_uart(chan, (const char *)ck, 2); - MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a message to send it over a serial byte stream - */ -MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) -{ - memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); - - uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); - - ck[0] = (uint8_t)(msg->checksum & 0xFF); - ck[1] = (uint8_t)(msg->checksum >> 8); - - return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; -} - -union __mavlink_bitfield { - uint8_t uint8; - int8_t int8; - uint16_t uint16; - int16_t int16; - uint32_t uint32; - int32_t int32; -}; - - -MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) -{ - crc_init(&msg->checksum); -} - -MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) -{ - crc_accumulate(c, &msg->checksum); -} - -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. Checksum and other failures will be silently - * ignored. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to barse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @return 0 if no message could be decoded, 1 else - * - * A typical use scenario of this function call is: - * - * @code - * #include // For fixed-width uint8_t type - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ -MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -{ - /* - default message crc function. You can override this per-system to - put this data in a different memory segment - */ -#if MAVLINK_CRC_EXTRA -#ifndef MAVLINK_MESSAGE_CRC - static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; -#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid] -#endif -#endif - -/* Enable this option to check the length of each message. - This allows invalid messages to be caught much sooner. Use if the transmission - medium is prone to missing (or extra) characters (e.g. a radio that fades in - and out). Only use if the channel will only contain messages types listed in - the headers. -*/ -#if MAVLINK_CHECK_MESSAGE_LENGTH -#ifndef MAVLINK_MESSAGE_LENGTH - static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; -#define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid] -#endif -#endif - - mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); ///< The currently decoded message - mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status - int bufferIndex = 0; - - status->msg_received = 0; - - switch (status->parse_state) - { - case MAVLINK_PARSE_STATE_UNINIT: - case MAVLINK_PARSE_STATE_IDLE: - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - rxmsg->magic = c; - mavlink_start_checksum(rxmsg); - } - break; - - case MAVLINK_PARSE_STATE_GOT_STX: - if (status->msg_received -/* Support shorter buffers than the - default maximum packet size */ -#if (MAVLINK_MAX_PAYLOAD_LEN < 255) - || c > MAVLINK_MAX_PAYLOAD_LEN -#endif - ) - { - status->buffer_overrun++; - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - } - else - { - // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 - rxmsg->len = c; - status->packet_idx = 0; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; - } - break; - - case MAVLINK_PARSE_STATE_GOT_LENGTH: - rxmsg->seq = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; - break; - - case MAVLINK_PARSE_STATE_GOT_SEQ: - rxmsg->sysid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; - break; - - case MAVLINK_PARSE_STATE_GOT_SYSID: - rxmsg->compid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; - break; - - case MAVLINK_PARSE_STATE_GOT_COMPID: -#if MAVLINK_CHECK_MESSAGE_LENGTH - if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c)) - { - status->parse_error++; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - break; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - } -#endif - rxmsg->msgid = c; - mavlink_update_checksum(rxmsg, c); - if (rxmsg->len == 0) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; - } - break; - - case MAVLINK_PARSE_STATE_GOT_MSGID: - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c; - mavlink_update_checksum(rxmsg, c); - if (status->packet_idx == rxmsg->len) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - break; - - case MAVLINK_PARSE_STATE_GOT_PAYLOAD: -#if MAVLINK_CRC_EXTRA - mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid)); -#endif - if (c != (rxmsg->checksum & 0xFF)) { - // Check first checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - mavlink_start_checksum(rxmsg); - } - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c; - } - break; - - case MAVLINK_PARSE_STATE_GOT_CRC1: - if (c != (rxmsg->checksum >> 8)) { - // Check second checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - mavlink_start_checksum(rxmsg); - } - } - else - { - // Successfully got message - status->msg_received = 1; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c; - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); - } - break; - } - - bufferIndex++; - // If a message has been sucessfully decoded, check index - if (status->msg_received == 1) - { - //while(status->current_seq != rxmsg->seq) - //{ - // status->packet_rx_drop_count++; - // status->current_seq++; - //} - status->current_rx_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; - } - - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; - r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; - r_mavlink_status->packet_rx_drop_count = status->parse_error; - status->parse_error = 0; - return status->msg_received; -} - -/** - * @brief Put a bitfield of length 1-32 bit into the buffer - * - * @param b the value to add, will be encoded in the bitfield - * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. - * @param packet_index the position in the packet (the index of the first byte to use) - * @param bit_index the position in the byte (the index of the first bit to use) - * @param buffer packet buffer to write into - * @return new position of the last used byte in the buffer - */ -MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) -{ - uint16_t bits_remain = bits; - // Transform number into network order - int32_t v; - uint8_t i_bit_index, i_byte_index, curr_bits_n; -#if MAVLINK_NEED_BYTE_SWAP - union { - int32_t i; - uint8_t b[4]; - } bin, bout; - bin.i = b; - bout.b[0] = bin.b[3]; - bout.b[1] = bin.b[2]; - bout.b[2] = bin.b[1]; - bout.b[3] = bin.b[0]; - v = bout.i; -#else - v = b; -#endif - - // buffer in - // 01100000 01000000 00000000 11110001 - // buffer out - // 11110001 00000000 01000000 01100000 - - // Existing partly filled byte (four free slots) - // 0111xxxx - - // Mask n free bits - // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 - // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 - - // Shift n bits into the right position - // out = in >> n; - - // Mask and shift bytes - i_bit_index = bit_index; - i_byte_index = packet_index; - if (bit_index > 0) - { - // If bits were available at start, they were available - // in the byte before the current index - i_byte_index--; - } - - // While bits have not been packed yet - while (bits_remain > 0) - { - // Bits still have to be packed - // there can be more than 8 bits, so - // we might have to pack them into more than one byte - - // First pack everything we can into the current 'open' byte - //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 - //FIXME - if (bits_remain <= (uint8_t)(8 - i_bit_index)) - { - // Enough space - curr_bits_n = (uint8_t)bits_remain; - } - else - { - curr_bits_n = (8 - i_bit_index); - } - - // Pack these n bits into the current byte - // Mask out whatever was at that position with ones (xxx11111) - buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); - // Put content to this position, by masking out the non-used part - buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v); - - // Increment the bit index - i_bit_index += curr_bits_n; - - // Now proceed to the next byte, if necessary - bits_remain -= curr_bits_n; - if (bits_remain > 0) - { - // Offer another 8 bits / one byte - i_byte_index++; - i_bit_index = 0; - } - } - - *r_bit_index = i_bit_index; - // If a partly filled byte is present, mark this as consumed - if (i_bit_index != 7) i_byte_index++; - return i_byte_index - packet_index; -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -// To make MAVLink work on your MCU, define comm_send_ch() if you wish -// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a -// whole packet at a time - -/* - -#include "mavlink_types.h" - -void comm_send_ch(mavlink_channel_t chan, uint8_t ch) -{ - if (chan == MAVLINK_COMM_0) - { - uart0_transmit(ch); - } - if (chan == MAVLINK_COMM_1) - { - uart1_transmit(ch); - } -} - */ - -MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len) -{ -#ifdef MAVLINK_SEND_UART_BYTES - /* this is the more efficient approach, if the platform - defines it */ - MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len); -#else - /* fallback to one byte at a time */ - uint16_t i; - for (i = 0; i < len; i++) { - comm_send_ch(chan, (uint8_t)buf[i]); - } -#endif -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - -#endif /* _MAVLINK_HELPERS_H_ */ diff --git a/mavlink/include/mavlink/v1.0/mavlink_protobuf_manager.hpp b/mavlink/include/mavlink/v1.0/mavlink_protobuf_manager.hpp deleted file mode 100644 index fd3ddd026..000000000 --- a/mavlink/include/mavlink/v1.0/mavlink_protobuf_manager.hpp +++ /dev/null @@ -1,377 +0,0 @@ -#ifndef MAVLINKPROTOBUFMANAGER_HPP -#define MAVLINKPROTOBUFMANAGER_HPP - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace mavlink -{ - -class ProtobufManager -{ -public: - ProtobufManager() - : mRegisteredTypeCount(0) - , mStreamID(0) - , mVerbose(false) - , kExtendedHeaderSize(MAVLINK_EXTENDED_HEADER_LEN) - , kExtendedPayloadMaxSize(MAVLINK_MAX_EXTENDED_PAYLOAD_LEN) - { - // register GLOverlay - { - std::tr1::shared_ptr msg(new px::GLOverlay); - registerType(msg); - } - - // register ObstacleList - { - std::tr1::shared_ptr msg(new px::ObstacleList); - registerType(msg); - } - - // register ObstacleMap - { - std::tr1::shared_ptr msg(new px::ObstacleMap); - registerType(msg); - } - - // register Path - { - std::tr1::shared_ptr msg(new px::Path); - registerType(msg); - } - - // register PointCloudXYZI - { - std::tr1::shared_ptr msg(new px::PointCloudXYZI); - registerType(msg); - } - - // register PointCloudXYZRGB - { - std::tr1::shared_ptr msg(new px::PointCloudXYZRGB); - registerType(msg); - } - - // register RGBDImage - { - std::tr1::shared_ptr msg(new px::RGBDImage); - registerType(msg); - } - - srand(time(NULL)); - mStreamID = rand() + 1; - } - - bool fragmentMessage(uint8_t system_id, uint8_t component_id, - uint8_t target_system, uint8_t target_component, - const google::protobuf::Message& protobuf_msg, - std::vector& fragments) const - { - TypeMap::const_iterator it = mTypeMap.find(protobuf_msg.GetTypeName()); - if (it == mTypeMap.end()) - { - std::cout << "# WARNING: Protobuf message with type " - << protobuf_msg.GetTypeName() << " is not registered." - << std::endl; - return false; - } - - uint8_t typecode = it->second; - - std::string data = protobuf_msg.SerializeAsString(); - - int fragmentCount = (protobuf_msg.ByteSize() + kExtendedPayloadMaxSize - 1) / kExtendedPayloadMaxSize; - unsigned int offset = 0; - - for (int i = 0; i < fragmentCount; ++i) - { - mavlink_extended_message_t fragment; - - // write extended header data - uint8_t* payload = reinterpret_cast(fragment.base_msg.payload64); - unsigned int length = 0; - uint8_t flags = 0; - - if (i < fragmentCount - 1) - { - length = kExtendedPayloadMaxSize; - flags |= 0x1; - } - else - { - length = protobuf_msg.ByteSize() - kExtendedPayloadMaxSize * (fragmentCount - 1); - } - - memcpy(payload, &target_system, 1); - memcpy(payload + 1, &target_component, 1); - memcpy(payload + 2, &typecode, 1); - memcpy(payload + 3, &length, 4); - memcpy(payload + 7, &mStreamID, 2); - memcpy(payload + 9, &offset, 4); - memcpy(payload + 13, &flags, 1); - - fragment.base_msg.msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE; - mavlink_finalize_message(&fragment.base_msg, system_id, component_id, kExtendedHeaderSize, 0); - - // write extended payload data - fragment.extended_payload_len = length; - memcpy(fragment.extended_payload, &data[offset], length); - - fragments.push_back(fragment); - offset += length; - } - - if (mVerbose) - { - std::cerr << "# INFO: Split extended message with size " - << protobuf_msg.ByteSize() << " into " - << fragmentCount << " fragments." << std::endl; - } - - return true; - } - - bool cacheFragment(mavlink_extended_message_t& msg) - { - if (!validFragment(msg)) - { - if (mVerbose) - { - std::cerr << "# WARNING: Message is not a valid fragment. " - << "Dropping message..." << std::endl; - } - return false; - } - - // read extended header - uint8_t* payload = reinterpret_cast(msg.base_msg.payload64); - uint8_t typecode = 0; - unsigned int length = 0; - unsigned short streamID = 0; - unsigned int offset = 0; - uint8_t flags = 0; - - memcpy(&typecode, payload + 2, 1); - memcpy(&length, payload + 3, 4); - memcpy(&streamID, payload + 7, 2); - memcpy(&offset, payload + 9, 4); - memcpy(&flags, payload + 13, 1); - - if (typecode >= mTypeMap.size()) - { - std::cout << "# WARNING: Protobuf message with type code " - << static_cast(typecode) << " is not registered." << std::endl; - return false; - } - - bool reassemble = false; - - FragmentQueue::iterator it = mFragmentQueue.find(streamID); - if (it == mFragmentQueue.end()) - { - if (offset == 0) - { - mFragmentQueue[streamID].push_back(msg); - - if ((flags & 0x1) != 0x1) - { - reassemble = true; - } - - if (mVerbose) - { - std::cerr << "# INFO: Added fragment to new queue." - << std::endl; - } - } - else - { - if (mVerbose) - { - std::cerr << "# WARNING: Message is not a valid fragment. " - << "Dropping message..." << std::endl; - } - } - } - else - { - std::deque& queue = it->second; - - if (queue.empty()) - { - if (offset == 0) - { - queue.push_back(msg); - - if ((flags & 0x1) != 0x1) - { - reassemble = true; - } - } - else - { - if (mVerbose) - { - std::cerr << "# WARNING: Message is not a valid fragment. " - << "Dropping message..." << std::endl; - } - } - } - else - { - if (fragmentDataSize(queue.back()) + fragmentOffset(queue.back()) != offset) - { - if (mVerbose) - { - std::cerr << "# WARNING: Previous fragment(s) have been lost. " - << "Dropping message and clearing queue..." << std::endl; - } - queue.clear(); - } - else - { - queue.push_back(msg); - - if ((flags & 0x1) != 0x1) - { - reassemble = true; - } - } - } - } - - if (reassemble) - { - std::deque& queue = mFragmentQueue[streamID]; - - std::string data; - for (size_t i = 0; i < queue.size(); ++i) - { - mavlink_extended_message_t& mavlink_msg = queue.at(i); - - data.append(reinterpret_cast(&mavlink_msg.extended_payload[0]), - static_cast(mavlink_msg.extended_payload_len)); - } - - mMessages.at(typecode)->ParseFromString(data); - - mMessageAvailable.at(typecode) = true; - - queue.clear(); - - if (mVerbose) - { - std::cerr << "# INFO: Reassembled fragments for message with typename " - << mMessages.at(typecode)->GetTypeName() << " and size " - << mMessages.at(typecode)->ByteSize() - << "." << std::endl; - } - } - - return true; - } - - bool getMessage(std::tr1::shared_ptr& msg) - { - for (size_t i = 0; i < mMessageAvailable.size(); ++i) - { - if (mMessageAvailable.at(i)) - { - msg = mMessages.at(i); - mMessageAvailable.at(i) = false; - - return true; - } - } - - return false; - } - -private: - void registerType(const std::tr1::shared_ptr& msg) - { - mTypeMap[msg->GetTypeName()] = mRegisteredTypeCount; - ++mRegisteredTypeCount; - mMessages.push_back(msg); - mMessageAvailable.push_back(false); - } - - bool validFragment(const mavlink_extended_message_t& msg) const - { - if (msg.base_msg.magic != MAVLINK_STX || - msg.base_msg.len != kExtendedHeaderSize || - msg.base_msg.msgid != MAVLINK_MSG_ID_EXTENDED_MESSAGE) - { - return false; - } - - uint16_t checksum; - checksum = crc_calculate(reinterpret_cast(&msg.base_msg.len), MAVLINK_CORE_HEADER_LEN); - crc_accumulate_buffer(&checksum, reinterpret_cast(&msg.base_msg.payload64), kExtendedHeaderSize); -#if MAVLINK_CRC_EXTRA - static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; - crc_accumulate(mavlink_message_crcs[msg.base_msg.msgid], &checksum); -#endif - - if (mavlink_ck_a(&(msg.base_msg)) != (uint8_t)(checksum & 0xFF) && - mavlink_ck_b(&(msg.base_msg)) != (uint8_t)(checksum >> 8)) - { - return false; - } - - return true; - } - - unsigned int fragmentDataSize(const mavlink_extended_message_t& msg) const - { - const uint8_t* payload = reinterpret_cast(msg.base_msg.payload64); - - return *(reinterpret_cast(payload + 3)); - } - - unsigned int fragmentOffset(const mavlink_extended_message_t& msg) const - { - const uint8_t* payload = reinterpret_cast(msg.base_msg.payload64); - - return *(reinterpret_cast(payload + 9)); - } - - int mRegisteredTypeCount; - unsigned short mStreamID; - bool mVerbose; - - typedef std::map TypeMap; - TypeMap mTypeMap; - std::vector< std::tr1::shared_ptr > mMessages; - std::vector mMessageAvailable; - - typedef std::map > FragmentQueue; - FragmentQueue mFragmentQueue; - - const int kExtendedHeaderSize; - /** - * Extended header structure - * ========================= - * byte 0 - target_system - * byte 1 - target_component - * byte 2 - extended message id (type code) - * bytes 3-6 - extended payload size in bytes - * byte 7-8 - stream ID - * byte 9-12 - fragment offset - * byte 13 - fragment flags (bit 0 - 1=more fragments, 0=last fragment) - */ - - const int kExtendedPayloadMaxSize; -}; - -} - -#endif diff --git a/mavlink/include/mavlink/v1.0/mavlink_types.h b/mavlink/include/mavlink/v1.0/mavlink_types.h deleted file mode 100644 index 43658e629..000000000 --- a/mavlink/include/mavlink/v1.0/mavlink_types.h +++ /dev/null @@ -1,161 +0,0 @@ -#ifndef MAVLINK_TYPES_H_ -#define MAVLINK_TYPES_H_ - -#include - -#ifndef MAVLINK_MAX_PAYLOAD_LEN -// it is possible to override this, but be careful! -#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length -#endif - -#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) -#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum -#define MAVLINK_NUM_CHECKSUM_BYTES 2 -#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) - -#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length - -#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255 -#define MAVLINK_EXTENDED_HEADER_LEN 14 - -#if (defined _MSC_VER) | ((defined __APPLE__) & (defined __MACH__)) | (defined __linux__) - /* full fledged 32bit++ OS */ - #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507 -#else - /* small microcontrollers */ - #define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048 -#endif - -#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) - -#pragma pack(push, 1) -typedef struct param_union { - union { - float param_float; - int32_t param_int32; - uint32_t param_uint32; - int16_t param_int16; - uint16_t param_uint16; - int8_t param_int8; - uint8_t param_uint8; - uint8_t bytes[4]; - }; - uint8_t type; -} mavlink_param_union_t; - -typedef struct __mavlink_system { - uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t type; ///< Unused, can be used by user to store the system's type - uint8_t state; ///< Unused, can be used by user to store the system's state - uint8_t mode; ///< Unused, can be used by user to store the system's mode - uint32_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode -} mavlink_system_t; - -typedef struct __mavlink_message { - uint16_t checksum; /// sent at end of packet - uint8_t magic; ///< protocol magic marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload - uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; -} mavlink_message_t; - -typedef struct __mavlink_extended_message { - mavlink_message_t base_msg; - int32_t extended_payload_len; ///< Length of extended payload if any - uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; -} mavlink_extended_message_t; -#pragma pack(pop) - -typedef enum { - MAVLINK_TYPE_CHAR = 0, - MAVLINK_TYPE_UINT8_T = 1, - MAVLINK_TYPE_INT8_T = 2, - MAVLINK_TYPE_UINT16_T = 3, - MAVLINK_TYPE_INT16_T = 4, - MAVLINK_TYPE_UINT32_T = 5, - MAVLINK_TYPE_INT32_T = 6, - MAVLINK_TYPE_UINT64_T = 7, - MAVLINK_TYPE_INT64_T = 8, - MAVLINK_TYPE_FLOAT = 9, - MAVLINK_TYPE_DOUBLE = 10 -} mavlink_message_type_t; - -#define MAVLINK_MAX_FIELDS 64 - -typedef struct __mavlink_field_info { - const char *name; // name of this field - const char *print_format; // printing format hint, or NULL - mavlink_message_type_t type; // type of this field - unsigned int array_length; // if non-zero, field is an array - unsigned int wire_offset; // offset of each field in the payload - unsigned int structure_offset; // offset in a C structure -} mavlink_field_info_t; - -// note that in this structure the order of fields is the order -// in the XML file, not necessary the wire order -typedef struct __mavlink_message_info { - const char *name; // name of the message - unsigned num_fields; // how many fields in this message - mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information -} mavlink_message_info_t; - -#define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0]))) -#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0]))) - -// checksum is immediately after the payload bytes -#define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) -#define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) - -typedef enum { - MAVLINK_COMM_0, - MAVLINK_COMM_1, - MAVLINK_COMM_2, - MAVLINK_COMM_3 -} mavlink_channel_t; - -/* - * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number - * of buffers they will use. If more are used, then the result will be - * a stack overrun - */ -#ifndef MAVLINK_COMM_NUM_BUFFERS -#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) -# define MAVLINK_COMM_NUM_BUFFERS 16 -#else -# define MAVLINK_COMM_NUM_BUFFERS 4 -#endif -#endif - -typedef enum { - MAVLINK_PARSE_STATE_UNINIT=0, - MAVLINK_PARSE_STATE_IDLE, - MAVLINK_PARSE_STATE_GOT_STX, - MAVLINK_PARSE_STATE_GOT_SEQ, - MAVLINK_PARSE_STATE_GOT_LENGTH, - MAVLINK_PARSE_STATE_GOT_SYSID, - MAVLINK_PARSE_STATE_GOT_COMPID, - MAVLINK_PARSE_STATE_GOT_MSGID, - MAVLINK_PARSE_STATE_GOT_PAYLOAD, - MAVLINK_PARSE_STATE_GOT_CRC1 -} mavlink_parse_state_t; ///< The state machine for the comm parser - -typedef struct __mavlink_status { - uint8_t msg_received; ///< Number of received messages - uint8_t buffer_overrun; ///< Number of buffer overruns - uint8_t parse_error; ///< Number of parse errors - mavlink_parse_state_t parse_state; ///< Parsing state machine - uint8_t packet_idx; ///< Index in current packet - uint8_t current_rx_seq; ///< Sequence number of last packet received - uint8_t current_tx_seq; ///< Sequence number of last packet sent - uint16_t packet_rx_success_count; ///< Received packets - uint16_t packet_rx_drop_count; ///< Number of packet drops -} mavlink_status_t; - -#define MAVLINK_BIG_ENDIAN 0 -#define MAVLINK_LITTLE_ENDIAN 1 - -#endif /* MAVLINK_TYPES_H_ */ diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink.h deleted file mode 100644 index f06223370..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink.h +++ /dev/null @@ -1,27 +0,0 @@ -/** @file - * @brief MAVLink comm protocol built from pixhawk.xml - * @see http://pixhawk.ethz.ch/software/mavlink - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#ifndef MAVLINK_STX -#define MAVLINK_STX 254 -#endif - -#ifndef MAVLINK_ENDIAN -#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN -#endif - -#ifndef MAVLINK_ALIGNED_FIELDS -#define MAVLINK_ALIGNED_FIELDS 1 -#endif - -#ifndef MAVLINK_CRC_EXTRA -#define MAVLINK_CRC_EXTRA 1 -#endif - -#include "version.h" -#include "pixhawk.h" - -#endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h deleted file mode 100644 index f1cac9f2c..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h +++ /dev/null @@ -1,401 +0,0 @@ -// MESSAGE ATTITUDE_CONTROL PACKING - -#define MAVLINK_MSG_ID_ATTITUDE_CONTROL 200 - -typedef struct __mavlink_attitude_control_t -{ - float roll; ///< roll - float pitch; ///< pitch - float yaw; ///< yaw - float thrust; ///< thrust - uint8_t target; ///< The system to be controlled - uint8_t roll_manual; ///< roll control enabled auto:0, manual:1 - uint8_t pitch_manual; ///< pitch auto:0, manual:1 - uint8_t yaw_manual; ///< yaw auto:0, manual:1 - uint8_t thrust_manual; ///< thrust auto:0, manual:1 -} mavlink_attitude_control_t; - -#define MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN 21 -#define MAVLINK_MSG_ID_200_LEN 21 - -#define MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC 254 -#define MAVLINK_MSG_ID_200_CRC 254 - - - -#define MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL { \ - "ATTITUDE_CONTROL", \ - 9, \ - { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_attitude_control_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_control_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_control_t, yaw) }, \ - { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_control_t, thrust) }, \ - { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_attitude_control_t, target) }, \ - { "roll_manual", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_attitude_control_t, roll_manual) }, \ - { "pitch_manual", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_attitude_control_t, pitch_manual) }, \ - { "yaw_manual", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_attitude_control_t, yaw_manual) }, \ - { "thrust_manual", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_attitude_control_t, thrust_manual) }, \ - } \ -} - - -/** - * @brief Pack a attitude_control message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target The system to be controlled - * @param roll roll - * @param pitch pitch - * @param yaw yaw - * @param thrust thrust - * @param roll_manual roll control enabled auto:0, manual:1 - * @param pitch_manual pitch auto:0, manual:1 - * @param yaw_manual yaw auto:0, manual:1 - * @param thrust_manual thrust auto:0, manual:1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target); - _mav_put_uint8_t(buf, 17, roll_manual); - _mav_put_uint8_t(buf, 18, pitch_manual); - _mav_put_uint8_t(buf, 19, yaw_manual); - _mav_put_uint8_t(buf, 20, thrust_manual); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#else - mavlink_attitude_control_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.target = target; - packet.roll_manual = roll_manual; - packet.pitch_manual = pitch_manual; - packet.yaw_manual = yaw_manual; - packet.thrust_manual = thrust_manual; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif -} - -/** - * @brief Pack a attitude_control message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target The system to be controlled - * @param roll roll - * @param pitch pitch - * @param yaw yaw - * @param thrust thrust - * @param roll_manual roll control enabled auto:0, manual:1 - * @param pitch_manual pitch auto:0, manual:1 - * @param yaw_manual yaw auto:0, manual:1 - * @param thrust_manual thrust auto:0, manual:1 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,float roll,float pitch,float yaw,float thrust,uint8_t roll_manual,uint8_t pitch_manual,uint8_t yaw_manual,uint8_t thrust_manual) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target); - _mav_put_uint8_t(buf, 17, roll_manual); - _mav_put_uint8_t(buf, 18, pitch_manual); - _mav_put_uint8_t(buf, 19, yaw_manual); - _mav_put_uint8_t(buf, 20, thrust_manual); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#else - mavlink_attitude_control_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.target = target; - packet.roll_manual = roll_manual; - packet.pitch_manual = pitch_manual; - packet.yaw_manual = yaw_manual; - packet.thrust_manual = thrust_manual; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif -} - -/** - * @brief Encode a attitude_control struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param attitude_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_control_t* attitude_control) -{ - return mavlink_msg_attitude_control_pack(system_id, component_id, msg, attitude_control->target, attitude_control->roll, attitude_control->pitch, attitude_control->yaw, attitude_control->thrust, attitude_control->roll_manual, attitude_control->pitch_manual, attitude_control->yaw_manual, attitude_control->thrust_manual); -} - -/** - * @brief Encode a attitude_control struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param attitude_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_attitude_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_control_t* attitude_control) -{ - return mavlink_msg_attitude_control_pack_chan(system_id, component_id, chan, msg, attitude_control->target, attitude_control->roll, attitude_control->pitch, attitude_control->yaw, attitude_control->thrust, attitude_control->roll_manual, attitude_control->pitch_manual, attitude_control->yaw_manual, attitude_control->thrust_manual); -} - -/** - * @brief Send a attitude_control message - * @param chan MAVLink channel to send the message - * - * @param target The system to be controlled - * @param roll roll - * @param pitch pitch - * @param yaw yaw - * @param thrust thrust - * @param roll_manual roll control enabled auto:0, manual:1 - * @param pitch_manual pitch auto:0, manual:1 - * @param yaw_manual yaw auto:0, manual:1 - * @param thrust_manual thrust auto:0, manual:1 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN]; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target); - _mav_put_uint8_t(buf, 17, roll_manual); - _mav_put_uint8_t(buf, 18, pitch_manual); - _mav_put_uint8_t(buf, 19, yaw_manual); - _mav_put_uint8_t(buf, 20, thrust_manual); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif -#else - mavlink_attitude_control_t packet; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.thrust = thrust; - packet.target = target; - packet.roll_manual = roll_manual; - packet.pitch_manual = pitch_manual; - packet.yaw_manual = yaw_manual; - packet.thrust_manual = thrust_manual; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_attitude_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, roll); - _mav_put_float(buf, 4, pitch); - _mav_put_float(buf, 8, yaw); - _mav_put_float(buf, 12, thrust); - _mav_put_uint8_t(buf, 16, target); - _mav_put_uint8_t(buf, 17, roll_manual); - _mav_put_uint8_t(buf, 18, pitch_manual); - _mav_put_uint8_t(buf, 19, yaw_manual); - _mav_put_uint8_t(buf, 20, thrust_manual); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif -#else - mavlink_attitude_control_t *packet = (mavlink_attitude_control_t *)msgbuf; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - packet->thrust = thrust; - packet->target = target; - packet->roll_manual = roll_manual; - packet->pitch_manual = pitch_manual; - packet->yaw_manual = yaw_manual; - packet->thrust_manual = thrust_manual; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE ATTITUDE_CONTROL UNPACKING - - -/** - * @brief Get field target from attitude_control message - * - * @return The system to be controlled - */ -static inline uint8_t mavlink_msg_attitude_control_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field roll from attitude_control message - * - * @return roll - */ -static inline float mavlink_msg_attitude_control_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field pitch from attitude_control message - * - * @return pitch - */ -static inline float mavlink_msg_attitude_control_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field yaw from attitude_control message - * - * @return yaw - */ -static inline float mavlink_msg_attitude_control_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field thrust from attitude_control message - * - * @return thrust - */ -static inline float mavlink_msg_attitude_control_get_thrust(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field roll_manual from attitude_control message - * - * @return roll control enabled auto:0, manual:1 - */ -static inline uint8_t mavlink_msg_attitude_control_get_roll_manual(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field pitch_manual from attitude_control message - * - * @return pitch auto:0, manual:1 - */ -static inline uint8_t mavlink_msg_attitude_control_get_pitch_manual(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 18); -} - -/** - * @brief Get field yaw_manual from attitude_control message - * - * @return yaw auto:0, manual:1 - */ -static inline uint8_t mavlink_msg_attitude_control_get_yaw_manual(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 19); -} - -/** - * @brief Get field thrust_manual from attitude_control message - * - * @return thrust auto:0, manual:1 - */ -static inline uint8_t mavlink_msg_attitude_control_get_thrust_manual(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 20); -} - -/** - * @brief Decode a attitude_control message into a struct - * - * @param msg The message to decode - * @param attitude_control C-struct to decode the message contents into - */ -static inline void mavlink_msg_attitude_control_decode(const mavlink_message_t* msg, mavlink_attitude_control_t* attitude_control) -{ -#if MAVLINK_NEED_BYTE_SWAP - attitude_control->roll = mavlink_msg_attitude_control_get_roll(msg); - attitude_control->pitch = mavlink_msg_attitude_control_get_pitch(msg); - attitude_control->yaw = mavlink_msg_attitude_control_get_yaw(msg); - attitude_control->thrust = mavlink_msg_attitude_control_get_thrust(msg); - attitude_control->target = mavlink_msg_attitude_control_get_target(msg); - attitude_control->roll_manual = mavlink_msg_attitude_control_get_roll_manual(msg); - attitude_control->pitch_manual = mavlink_msg_attitude_control_get_pitch_manual(msg); - attitude_control->yaw_manual = mavlink_msg_attitude_control_get_yaw_manual(msg); - attitude_control->thrust_manual = mavlink_msg_attitude_control_get_thrust_manual(msg); -#else - memcpy(attitude_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h deleted file mode 100644 index 7bc27982a..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h +++ /dev/null @@ -1,369 +0,0 @@ -// MESSAGE BRIEF_FEATURE PACKING - -#define MAVLINK_MSG_ID_BRIEF_FEATURE 195 - -typedef struct __mavlink_brief_feature_t -{ - float x; ///< x position in m - float y; ///< y position in m - float z; ///< z position in m - float response; ///< Harris operator response at this location - uint16_t size; ///< Size in pixels - uint16_t orientation; ///< Orientation - uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true - uint8_t descriptor[32]; ///< Descriptor -} mavlink_brief_feature_t; - -#define MAVLINK_MSG_ID_BRIEF_FEATURE_LEN 53 -#define MAVLINK_MSG_ID_195_LEN 53 - -#define MAVLINK_MSG_ID_BRIEF_FEATURE_CRC 88 -#define MAVLINK_MSG_ID_195_CRC 88 - -#define MAVLINK_MSG_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32 - -#define MAVLINK_MESSAGE_INFO_BRIEF_FEATURE { \ - "BRIEF_FEATURE", \ - 8, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_brief_feature_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_brief_feature_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_brief_feature_t, z) }, \ - { "response", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_brief_feature_t, response) }, \ - { "size", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_brief_feature_t, size) }, \ - { "orientation", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_brief_feature_t, orientation) }, \ - { "orientation_assignment", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_brief_feature_t, orientation_assignment) }, \ - { "descriptor", NULL, MAVLINK_TYPE_UINT8_T, 32, 21, offsetof(mavlink_brief_feature_t, descriptor) }, \ - } \ -} - - -/** - * @brief Pack a brief_feature message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param x x position in m - * @param y y position in m - * @param z z position in m - * @param orientation_assignment Orientation assignment 0: false, 1:true - * @param size Size in pixels - * @param orientation Orientation - * @param descriptor Descriptor - * @param response Harris operator response at this location - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t *descriptor, float response) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_BRIEF_FEATURE_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, response); - _mav_put_uint16_t(buf, 16, size); - _mav_put_uint16_t(buf, 18, orientation); - _mav_put_uint8_t(buf, 20, orientation_assignment); - _mav_put_uint8_t_array(buf, 21, descriptor, 32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#else - mavlink_brief_feature_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.response = response; - packet.size = size; - packet.orientation = orientation; - packet.orientation_assignment = orientation_assignment; - mav_array_memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif -} - -/** - * @brief Pack a brief_feature message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param x x position in m - * @param y y position in m - * @param z z position in m - * @param orientation_assignment Orientation assignment 0: false, 1:true - * @param size Size in pixels - * @param orientation Orientation - * @param descriptor Descriptor - * @param response Harris operator response at this location - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float x,float y,float z,uint8_t orientation_assignment,uint16_t size,uint16_t orientation,const uint8_t *descriptor,float response) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_BRIEF_FEATURE_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, response); - _mav_put_uint16_t(buf, 16, size); - _mav_put_uint16_t(buf, 18, orientation); - _mav_put_uint8_t(buf, 20, orientation_assignment); - _mav_put_uint8_t_array(buf, 21, descriptor, 32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#else - mavlink_brief_feature_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.response = response; - packet.size = size; - packet.orientation = orientation; - packet.orientation_assignment = orientation_assignment; - mav_array_memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif -} - -/** - * @brief Encode a brief_feature struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param brief_feature C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_brief_feature_t* brief_feature) -{ - return mavlink_msg_brief_feature_pack(system_id, component_id, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response); -} - -/** - * @brief Encode a brief_feature struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param brief_feature C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_brief_feature_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_brief_feature_t* brief_feature) -{ - return mavlink_msg_brief_feature_pack_chan(system_id, component_id, chan, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response); -} - -/** - * @brief Send a brief_feature message - * @param chan MAVLink channel to send the message - * - * @param x x position in m - * @param y y position in m - * @param z z position in m - * @param orientation_assignment Orientation assignment 0: false, 1:true - * @param size Size in pixels - * @param orientation Orientation - * @param descriptor Descriptor - * @param response Harris operator response at this location - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t *descriptor, float response) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_BRIEF_FEATURE_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, response); - _mav_put_uint16_t(buf, 16, size); - _mav_put_uint16_t(buf, 18, orientation); - _mav_put_uint8_t(buf, 20, orientation_assignment); - _mav_put_uint8_t_array(buf, 21, descriptor, 32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif -#else - mavlink_brief_feature_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.response = response; - packet.size = size; - packet.orientation = orientation; - packet.orientation_assignment = orientation_assignment; - mav_array_memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)&packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)&packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_BRIEF_FEATURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_brief_feature_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t *descriptor, float response) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, response); - _mav_put_uint16_t(buf, 16, size); - _mav_put_uint16_t(buf, 18, orientation); - _mav_put_uint8_t(buf, 20, orientation_assignment); - _mav_put_uint8_t_array(buf, 21, descriptor, 32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif -#else - mavlink_brief_feature_t *packet = (mavlink_brief_feature_t *)msgbuf; - packet->x = x; - packet->y = y; - packet->z = z; - packet->response = response; - packet->size = size; - packet->orientation = orientation; - packet->orientation_assignment = orientation_assignment; - mav_array_memcpy(packet->descriptor, descriptor, sizeof(uint8_t)*32); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE BRIEF_FEATURE UNPACKING - - -/** - * @brief Get field x from brief_feature message - * - * @return x position in m - */ -static inline float mavlink_msg_brief_feature_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from brief_feature message - * - * @return y position in m - */ -static inline float mavlink_msg_brief_feature_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from brief_feature message - * - * @return z position in m - */ -static inline float mavlink_msg_brief_feature_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field orientation_assignment from brief_feature message - * - * @return Orientation assignment 0: false, 1:true - */ -static inline uint8_t mavlink_msg_brief_feature_get_orientation_assignment(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 20); -} - -/** - * @brief Get field size from brief_feature message - * - * @return Size in pixels - */ -static inline uint16_t mavlink_msg_brief_feature_get_size(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field orientation from brief_feature message - * - * @return Orientation - */ -static inline uint16_t mavlink_msg_brief_feature_get_orientation(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 18); -} - -/** - * @brief Get field descriptor from brief_feature message - * - * @return Descriptor - */ -static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, uint8_t *descriptor) -{ - return _MAV_RETURN_uint8_t_array(msg, descriptor, 32, 21); -} - -/** - * @brief Get field response from brief_feature message - * - * @return Harris operator response at this location - */ -static inline float mavlink_msg_brief_feature_get_response(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a brief_feature message into a struct - * - * @param msg The message to decode - * @param brief_feature C-struct to decode the message contents into - */ -static inline void mavlink_msg_brief_feature_decode(const mavlink_message_t* msg, mavlink_brief_feature_t* brief_feature) -{ -#if MAVLINK_NEED_BYTE_SWAP - brief_feature->x = mavlink_msg_brief_feature_get_x(msg); - brief_feature->y = mavlink_msg_brief_feature_get_y(msg); - brief_feature->z = mavlink_msg_brief_feature_get_z(msg); - brief_feature->response = mavlink_msg_brief_feature_get_response(msg); - brief_feature->size = mavlink_msg_brief_feature_get_size(msg); - brief_feature->orientation = mavlink_msg_brief_feature_get_orientation(msg); - brief_feature->orientation_assignment = mavlink_msg_brief_feature_get_orientation_assignment(msg); - mavlink_msg_brief_feature_get_descriptor(msg, brief_feature->descriptor); -#else - memcpy(brief_feature, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h deleted file mode 100644 index 37270d49c..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h +++ /dev/null @@ -1,737 +0,0 @@ -// MESSAGE IMAGE_AVAILABLE PACKING - -#define MAVLINK_MSG_ID_IMAGE_AVAILABLE 154 - -typedef struct __mavlink_image_available_t -{ - uint64_t cam_id; ///< Camera id - uint64_t timestamp; ///< Timestamp - uint64_t valid_until; ///< Until which timestamp this buffer will stay valid - uint32_t img_seq; ///< The image sequence number - uint32_t img_buf_index; ///< Position of the image in the buffer, starts with 0 - uint32_t key; ///< Shared memory area key - uint32_t exposure; ///< Exposure time, in microseconds - float gain; ///< Camera gain - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad - float local_z; ///< Local frame Z coordinate (height over ground) - float lat; ///< GPS X coordinate - float lon; ///< GPS Y coordinate - float alt; ///< Global frame altitude - float ground_x; ///< Ground truth X - float ground_y; ///< Ground truth Y - float ground_z; ///< Ground truth Z - uint16_t width; ///< Image width - uint16_t height; ///< Image height - uint16_t depth; ///< Image depth - uint8_t cam_no; ///< Camera # (starts with 0) - uint8_t channels; ///< Image channels -} mavlink_image_available_t; - -#define MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN 92 -#define MAVLINK_MSG_ID_154_LEN 92 - -#define MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC 224 -#define MAVLINK_MSG_ID_154_CRC 224 - - - -#define MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE { \ - "IMAGE_AVAILABLE", \ - 23, \ - { { "cam_id", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_image_available_t, cam_id) }, \ - { "timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_image_available_t, timestamp) }, \ - { "valid_until", NULL, MAVLINK_TYPE_UINT64_T, 0, 16, offsetof(mavlink_image_available_t, valid_until) }, \ - { "img_seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_image_available_t, img_seq) }, \ - { "img_buf_index", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_image_available_t, img_buf_index) }, \ - { "key", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_image_available_t, key) }, \ - { "exposure", NULL, MAVLINK_TYPE_UINT32_T, 0, 36, offsetof(mavlink_image_available_t, exposure) }, \ - { "gain", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_image_available_t, gain) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_image_available_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_image_available_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_image_available_t, yaw) }, \ - { "local_z", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_image_available_t, local_z) }, \ - { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_image_available_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_image_available_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_image_available_t, alt) }, \ - { "ground_x", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_image_available_t, ground_x) }, \ - { "ground_y", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_image_available_t, ground_y) }, \ - { "ground_z", NULL, MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_image_available_t, ground_z) }, \ - { "width", NULL, MAVLINK_TYPE_UINT16_T, 0, 84, offsetof(mavlink_image_available_t, width) }, \ - { "height", NULL, MAVLINK_TYPE_UINT16_T, 0, 86, offsetof(mavlink_image_available_t, height) }, \ - { "depth", NULL, MAVLINK_TYPE_UINT16_T, 0, 88, offsetof(mavlink_image_available_t, depth) }, \ - { "cam_no", NULL, MAVLINK_TYPE_UINT8_T, 0, 90, offsetof(mavlink_image_available_t, cam_no) }, \ - { "channels", NULL, MAVLINK_TYPE_UINT8_T, 0, 91, offsetof(mavlink_image_available_t, channels) }, \ - } \ -} - - -/** - * @brief Pack a image_available message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param cam_id Camera id - * @param cam_no Camera # (starts with 0) - * @param timestamp Timestamp - * @param valid_until Until which timestamp this buffer will stay valid - * @param img_seq The image sequence number - * @param img_buf_index Position of the image in the buffer, starts with 0 - * @param width Image width - * @param height Image height - * @param depth Image depth - * @param channels Image channels - * @param key Shared memory area key - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN]; - _mav_put_uint64_t(buf, 0, cam_id); - _mav_put_uint64_t(buf, 8, timestamp); - _mav_put_uint64_t(buf, 16, valid_until); - _mav_put_uint32_t(buf, 24, img_seq); - _mav_put_uint32_t(buf, 28, img_buf_index); - _mav_put_uint32_t(buf, 32, key); - _mav_put_uint32_t(buf, 36, exposure); - _mav_put_float(buf, 40, gain); - _mav_put_float(buf, 44, roll); - _mav_put_float(buf, 48, pitch); - _mav_put_float(buf, 52, yaw); - _mav_put_float(buf, 56, local_z); - _mav_put_float(buf, 60, lat); - _mav_put_float(buf, 64, lon); - _mav_put_float(buf, 68, alt); - _mav_put_float(buf, 72, ground_x); - _mav_put_float(buf, 76, ground_y); - _mav_put_float(buf, 80, ground_z); - _mav_put_uint16_t(buf, 84, width); - _mav_put_uint16_t(buf, 86, height); - _mav_put_uint16_t(buf, 88, depth); - _mav_put_uint8_t(buf, 90, cam_no); - _mav_put_uint8_t(buf, 91, channels); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#else - mavlink_image_available_t packet; - packet.cam_id = cam_id; - packet.timestamp = timestamp; - packet.valid_until = valid_until; - packet.img_seq = img_seq; - packet.img_buf_index = img_buf_index; - packet.key = key; - packet.exposure = exposure; - packet.gain = gain; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.local_z = local_z; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.ground_x = ground_x; - packet.ground_y = ground_y; - packet.ground_z = ground_z; - packet.width = width; - packet.height = height; - packet.depth = depth; - packet.cam_no = cam_no; - packet.channels = channels; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif -} - -/** - * @brief Pack a image_available message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param cam_id Camera id - * @param cam_no Camera # (starts with 0) - * @param timestamp Timestamp - * @param valid_until Until which timestamp this buffer will stay valid - * @param img_seq The image sequence number - * @param img_buf_index Position of the image in the buffer, starts with 0 - * @param width Image width - * @param height Image height - * @param depth Image depth - * @param channels Image channels - * @param key Shared memory area key - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t cam_id,uint8_t cam_no,uint64_t timestamp,uint64_t valid_until,uint32_t img_seq,uint32_t img_buf_index,uint16_t width,uint16_t height,uint16_t depth,uint8_t channels,uint32_t key,uint32_t exposure,float gain,float roll,float pitch,float yaw,float local_z,float lat,float lon,float alt,float ground_x,float ground_y,float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN]; - _mav_put_uint64_t(buf, 0, cam_id); - _mav_put_uint64_t(buf, 8, timestamp); - _mav_put_uint64_t(buf, 16, valid_until); - _mav_put_uint32_t(buf, 24, img_seq); - _mav_put_uint32_t(buf, 28, img_buf_index); - _mav_put_uint32_t(buf, 32, key); - _mav_put_uint32_t(buf, 36, exposure); - _mav_put_float(buf, 40, gain); - _mav_put_float(buf, 44, roll); - _mav_put_float(buf, 48, pitch); - _mav_put_float(buf, 52, yaw); - _mav_put_float(buf, 56, local_z); - _mav_put_float(buf, 60, lat); - _mav_put_float(buf, 64, lon); - _mav_put_float(buf, 68, alt); - _mav_put_float(buf, 72, ground_x); - _mav_put_float(buf, 76, ground_y); - _mav_put_float(buf, 80, ground_z); - _mav_put_uint16_t(buf, 84, width); - _mav_put_uint16_t(buf, 86, height); - _mav_put_uint16_t(buf, 88, depth); - _mav_put_uint8_t(buf, 90, cam_no); - _mav_put_uint8_t(buf, 91, channels); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#else - mavlink_image_available_t packet; - packet.cam_id = cam_id; - packet.timestamp = timestamp; - packet.valid_until = valid_until; - packet.img_seq = img_seq; - packet.img_buf_index = img_buf_index; - packet.key = key; - packet.exposure = exposure; - packet.gain = gain; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.local_z = local_z; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.ground_x = ground_x; - packet.ground_y = ground_y; - packet.ground_z = ground_z; - packet.width = width; - packet.height = height; - packet.depth = depth; - packet.cam_no = cam_no; - packet.channels = channels; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif -} - -/** - * @brief Encode a image_available struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param image_available C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_available_t* image_available) -{ - return mavlink_msg_image_available_pack(system_id, component_id, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z); -} - -/** - * @brief Encode a image_available struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param image_available C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_available_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_available_t* image_available) -{ - return mavlink_msg_image_available_pack_chan(system_id, component_id, chan, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z); -} - -/** - * @brief Send a image_available message - * @param chan MAVLink channel to send the message - * - * @param cam_id Camera id - * @param cam_no Camera # (starts with 0) - * @param timestamp Timestamp - * @param valid_until Until which timestamp this buffer will stay valid - * @param img_seq The image sequence number - * @param img_buf_index Position of the image in the buffer, starts with 0 - * @param width Image width - * @param height Image height - * @param depth Image depth - * @param channels Image channels - * @param key Shared memory area key - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN]; - _mav_put_uint64_t(buf, 0, cam_id); - _mav_put_uint64_t(buf, 8, timestamp); - _mav_put_uint64_t(buf, 16, valid_until); - _mav_put_uint32_t(buf, 24, img_seq); - _mav_put_uint32_t(buf, 28, img_buf_index); - _mav_put_uint32_t(buf, 32, key); - _mav_put_uint32_t(buf, 36, exposure); - _mav_put_float(buf, 40, gain); - _mav_put_float(buf, 44, roll); - _mav_put_float(buf, 48, pitch); - _mav_put_float(buf, 52, yaw); - _mav_put_float(buf, 56, local_z); - _mav_put_float(buf, 60, lat); - _mav_put_float(buf, 64, lon); - _mav_put_float(buf, 68, alt); - _mav_put_float(buf, 72, ground_x); - _mav_put_float(buf, 76, ground_y); - _mav_put_float(buf, 80, ground_z); - _mav_put_uint16_t(buf, 84, width); - _mav_put_uint16_t(buf, 86, height); - _mav_put_uint16_t(buf, 88, depth); - _mav_put_uint8_t(buf, 90, cam_no); - _mav_put_uint8_t(buf, 91, channels); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif -#else - mavlink_image_available_t packet; - packet.cam_id = cam_id; - packet.timestamp = timestamp; - packet.valid_until = valid_until; - packet.img_seq = img_seq; - packet.img_buf_index = img_buf_index; - packet.key = key; - packet.exposure = exposure; - packet.gain = gain; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.local_z = local_z; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.ground_x = ground_x; - packet.ground_y = ground_y; - packet.ground_z = ground_z; - packet.width = width; - packet.height = height; - packet.depth = depth; - packet.cam_no = cam_no; - packet.channels = channels; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)&packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)&packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_image_available_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, cam_id); - _mav_put_uint64_t(buf, 8, timestamp); - _mav_put_uint64_t(buf, 16, valid_until); - _mav_put_uint32_t(buf, 24, img_seq); - _mav_put_uint32_t(buf, 28, img_buf_index); - _mav_put_uint32_t(buf, 32, key); - _mav_put_uint32_t(buf, 36, exposure); - _mav_put_float(buf, 40, gain); - _mav_put_float(buf, 44, roll); - _mav_put_float(buf, 48, pitch); - _mav_put_float(buf, 52, yaw); - _mav_put_float(buf, 56, local_z); - _mav_put_float(buf, 60, lat); - _mav_put_float(buf, 64, lon); - _mav_put_float(buf, 68, alt); - _mav_put_float(buf, 72, ground_x); - _mav_put_float(buf, 76, ground_y); - _mav_put_float(buf, 80, ground_z); - _mav_put_uint16_t(buf, 84, width); - _mav_put_uint16_t(buf, 86, height); - _mav_put_uint16_t(buf, 88, depth); - _mav_put_uint8_t(buf, 90, cam_no); - _mav_put_uint8_t(buf, 91, channels); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif -#else - mavlink_image_available_t *packet = (mavlink_image_available_t *)msgbuf; - packet->cam_id = cam_id; - packet->timestamp = timestamp; - packet->valid_until = valid_until; - packet->img_seq = img_seq; - packet->img_buf_index = img_buf_index; - packet->key = key; - packet->exposure = exposure; - packet->gain = gain; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - packet->local_z = local_z; - packet->lat = lat; - packet->lon = lon; - packet->alt = alt; - packet->ground_x = ground_x; - packet->ground_y = ground_y; - packet->ground_z = ground_z; - packet->width = width; - packet->height = height; - packet->depth = depth; - packet->cam_no = cam_no; - packet->channels = channels; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE IMAGE_AVAILABLE UNPACKING - - -/** - * @brief Get field cam_id from image_available message - * - * @return Camera id - */ -static inline uint64_t mavlink_msg_image_available_get_cam_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field cam_no from image_available message - * - * @return Camera # (starts with 0) - */ -static inline uint8_t mavlink_msg_image_available_get_cam_no(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 90); -} - -/** - * @brief Get field timestamp from image_available message - * - * @return Timestamp - */ -static inline uint64_t mavlink_msg_image_available_get_timestamp(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 8); -} - -/** - * @brief Get field valid_until from image_available message - * - * @return Until which timestamp this buffer will stay valid - */ -static inline uint64_t mavlink_msg_image_available_get_valid_until(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 16); -} - -/** - * @brief Get field img_seq from image_available message - * - * @return The image sequence number - */ -static inline uint32_t mavlink_msg_image_available_get_img_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 24); -} - -/** - * @brief Get field img_buf_index from image_available message - * - * @return Position of the image in the buffer, starts with 0 - */ -static inline uint32_t mavlink_msg_image_available_get_img_buf_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 28); -} - -/** - * @brief Get field width from image_available message - * - * @return Image width - */ -static inline uint16_t mavlink_msg_image_available_get_width(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 84); -} - -/** - * @brief Get field height from image_available message - * - * @return Image height - */ -static inline uint16_t mavlink_msg_image_available_get_height(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 86); -} - -/** - * @brief Get field depth from image_available message - * - * @return Image depth - */ -static inline uint16_t mavlink_msg_image_available_get_depth(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 88); -} - -/** - * @brief Get field channels from image_available message - * - * @return Image channels - */ -static inline uint8_t mavlink_msg_image_available_get_channels(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 91); -} - -/** - * @brief Get field key from image_available message - * - * @return Shared memory area key - */ -static inline uint32_t mavlink_msg_image_available_get_key(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 32); -} - -/** - * @brief Get field exposure from image_available message - * - * @return Exposure time, in microseconds - */ -static inline uint32_t mavlink_msg_image_available_get_exposure(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 36); -} - -/** - * @brief Get field gain from image_available message - * - * @return Camera gain - */ -static inline float mavlink_msg_image_available_get_gain(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 40); -} - -/** - * @brief Get field roll from image_available message - * - * @return Roll angle in rad - */ -static inline float mavlink_msg_image_available_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 44); -} - -/** - * @brief Get field pitch from image_available message - * - * @return Pitch angle in rad - */ -static inline float mavlink_msg_image_available_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 48); -} - -/** - * @brief Get field yaw from image_available message - * - * @return Yaw angle in rad - */ -static inline float mavlink_msg_image_available_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 52); -} - -/** - * @brief Get field local_z from image_available message - * - * @return Local frame Z coordinate (height over ground) - */ -static inline float mavlink_msg_image_available_get_local_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 56); -} - -/** - * @brief Get field lat from image_available message - * - * @return GPS X coordinate - */ -static inline float mavlink_msg_image_available_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 60); -} - -/** - * @brief Get field lon from image_available message - * - * @return GPS Y coordinate - */ -static inline float mavlink_msg_image_available_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 64); -} - -/** - * @brief Get field alt from image_available message - * - * @return Global frame altitude - */ -static inline float mavlink_msg_image_available_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 68); -} - -/** - * @brief Get field ground_x from image_available message - * - * @return Ground truth X - */ -static inline float mavlink_msg_image_available_get_ground_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 72); -} - -/** - * @brief Get field ground_y from image_available message - * - * @return Ground truth Y - */ -static inline float mavlink_msg_image_available_get_ground_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 76); -} - -/** - * @brief Get field ground_z from image_available message - * - * @return Ground truth Z - */ -static inline float mavlink_msg_image_available_get_ground_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 80); -} - -/** - * @brief Decode a image_available message into a struct - * - * @param msg The message to decode - * @param image_available C-struct to decode the message contents into - */ -static inline void mavlink_msg_image_available_decode(const mavlink_message_t* msg, mavlink_image_available_t* image_available) -{ -#if MAVLINK_NEED_BYTE_SWAP - image_available->cam_id = mavlink_msg_image_available_get_cam_id(msg); - image_available->timestamp = mavlink_msg_image_available_get_timestamp(msg); - image_available->valid_until = mavlink_msg_image_available_get_valid_until(msg); - image_available->img_seq = mavlink_msg_image_available_get_img_seq(msg); - image_available->img_buf_index = mavlink_msg_image_available_get_img_buf_index(msg); - image_available->key = mavlink_msg_image_available_get_key(msg); - image_available->exposure = mavlink_msg_image_available_get_exposure(msg); - image_available->gain = mavlink_msg_image_available_get_gain(msg); - image_available->roll = mavlink_msg_image_available_get_roll(msg); - image_available->pitch = mavlink_msg_image_available_get_pitch(msg); - image_available->yaw = mavlink_msg_image_available_get_yaw(msg); - image_available->local_z = mavlink_msg_image_available_get_local_z(msg); - image_available->lat = mavlink_msg_image_available_get_lat(msg); - image_available->lon = mavlink_msg_image_available_get_lon(msg); - image_available->alt = mavlink_msg_image_available_get_alt(msg); - image_available->ground_x = mavlink_msg_image_available_get_ground_x(msg); - image_available->ground_y = mavlink_msg_image_available_get_ground_y(msg); - image_available->ground_z = mavlink_msg_image_available_get_ground_z(msg); - image_available->width = mavlink_msg_image_available_get_width(msg); - image_available->height = mavlink_msg_image_available_get_height(msg); - image_available->depth = mavlink_msg_image_available_get_depth(msg); - image_available->cam_no = mavlink_msg_image_available_get_cam_no(msg); - image_available->channels = mavlink_msg_image_available_get_channels(msg); -#else - memcpy(image_available, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h deleted file mode 100644 index 85f8eff55..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h +++ /dev/null @@ -1,209 +0,0 @@ -// MESSAGE IMAGE_TRIGGER_CONTROL PACKING - -#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL 153 - -typedef struct __mavlink_image_trigger_control_t -{ - uint8_t enable; ///< 0 to disable, 1 to enable -} mavlink_image_trigger_control_t; - -#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN 1 -#define MAVLINK_MSG_ID_153_LEN 1 - -#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC 95 -#define MAVLINK_MSG_ID_153_CRC 95 - - - -#define MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL { \ - "IMAGE_TRIGGER_CONTROL", \ - 1, \ - { { "enable", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_image_trigger_control_t, enable) }, \ - } \ -} - - -/** - * @brief Pack a image_trigger_control message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param enable 0 to disable, 1 to enable - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t enable) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN]; - _mav_put_uint8_t(buf, 0, enable); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#else - mavlink_image_trigger_control_t packet; - packet.enable = enable; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif -} - -/** - * @brief Pack a image_trigger_control message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param enable 0 to disable, 1 to enable - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t enable) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN]; - _mav_put_uint8_t(buf, 0, enable); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#else - mavlink_image_trigger_control_t packet; - packet.enable = enable; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif -} - -/** - * @brief Encode a image_trigger_control struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param image_trigger_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_trigger_control_t* image_trigger_control) -{ - return mavlink_msg_image_trigger_control_pack(system_id, component_id, msg, image_trigger_control->enable); -} - -/** - * @brief Encode a image_trigger_control struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param image_trigger_control C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_trigger_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_trigger_control_t* image_trigger_control) -{ - return mavlink_msg_image_trigger_control_pack_chan(system_id, component_id, chan, msg, image_trigger_control->enable); -} - -/** - * @brief Send a image_trigger_control message - * @param chan MAVLink channel to send the message - * - * @param enable 0 to disable, 1 to enable - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN]; - _mav_put_uint8_t(buf, 0, enable); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif -#else - mavlink_image_trigger_control_t packet; - packet.enable = enable; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_image_trigger_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t enable) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, enable); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif -#else - mavlink_image_trigger_control_t *packet = (mavlink_image_trigger_control_t *)msgbuf; - packet->enable = enable; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE IMAGE_TRIGGER_CONTROL UNPACKING - - -/** - * @brief Get field enable from image_trigger_control message - * - * @return 0 to disable, 1 to enable - */ -static inline uint8_t mavlink_msg_image_trigger_control_get_enable(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Decode a image_trigger_control message into a struct - * - * @param msg The message to decode - * @param image_trigger_control C-struct to decode the message contents into - */ -static inline void mavlink_msg_image_trigger_control_decode(const mavlink_message_t* msg, mavlink_image_trigger_control_t* image_trigger_control) -{ -#if MAVLINK_NEED_BYTE_SWAP - image_trigger_control->enable = mavlink_msg_image_trigger_control_get_enable(msg); -#else - memcpy(image_trigger_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h deleted file mode 100644 index b438c74ea..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h +++ /dev/null @@ -1,473 +0,0 @@ -// MESSAGE IMAGE_TRIGGERED PACKING - -#define MAVLINK_MSG_ID_IMAGE_TRIGGERED 152 - -typedef struct __mavlink_image_triggered_t -{ - uint64_t timestamp; ///< Timestamp - uint32_t seq; ///< IMU seq - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad - float local_z; ///< Local frame Z coordinate (height over ground) - float lat; ///< GPS X coordinate - float lon; ///< GPS Y coordinate - float alt; ///< Global frame altitude - float ground_x; ///< Ground truth X - float ground_y; ///< Ground truth Y - float ground_z; ///< Ground truth Z -} mavlink_image_triggered_t; - -#define MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN 52 -#define MAVLINK_MSG_ID_152_LEN 52 - -#define MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC 86 -#define MAVLINK_MSG_ID_152_CRC 86 - - - -#define MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED { \ - "IMAGE_TRIGGERED", \ - 12, \ - { { "timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_image_triggered_t, timestamp) }, \ - { "seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_image_triggered_t, seq) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_image_triggered_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_image_triggered_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_image_triggered_t, yaw) }, \ - { "local_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_image_triggered_t, local_z) }, \ - { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_image_triggered_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_image_triggered_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_image_triggered_t, alt) }, \ - { "ground_x", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_image_triggered_t, ground_x) }, \ - { "ground_y", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_image_triggered_t, ground_y) }, \ - { "ground_z", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_image_triggered_t, ground_z) }, \ - } \ -} - - -/** - * @brief Pack a image_triggered message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param timestamp Timestamp - * @param seq IMU seq - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN]; - _mav_put_uint64_t(buf, 0, timestamp); - _mav_put_uint32_t(buf, 8, seq); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_float(buf, 24, local_z); - _mav_put_float(buf, 28, lat); - _mav_put_float(buf, 32, lon); - _mav_put_float(buf, 36, alt); - _mav_put_float(buf, 40, ground_x); - _mav_put_float(buf, 44, ground_y); - _mav_put_float(buf, 48, ground_z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#else - mavlink_image_triggered_t packet; - packet.timestamp = timestamp; - packet.seq = seq; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.local_z = local_z; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.ground_x = ground_x; - packet.ground_y = ground_y; - packet.ground_z = ground_z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif -} - -/** - * @brief Pack a image_triggered message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param timestamp Timestamp - * @param seq IMU seq - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t timestamp,uint32_t seq,float roll,float pitch,float yaw,float local_z,float lat,float lon,float alt,float ground_x,float ground_y,float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN]; - _mav_put_uint64_t(buf, 0, timestamp); - _mav_put_uint32_t(buf, 8, seq); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_float(buf, 24, local_z); - _mav_put_float(buf, 28, lat); - _mav_put_float(buf, 32, lon); - _mav_put_float(buf, 36, alt); - _mav_put_float(buf, 40, ground_x); - _mav_put_float(buf, 44, ground_y); - _mav_put_float(buf, 48, ground_z); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#else - mavlink_image_triggered_t packet; - packet.timestamp = timestamp; - packet.seq = seq; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.local_z = local_z; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.ground_x = ground_x; - packet.ground_y = ground_y; - packet.ground_z = ground_z; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif -} - -/** - * @brief Encode a image_triggered struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param image_triggered C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_triggered_t* image_triggered) -{ - return mavlink_msg_image_triggered_pack(system_id, component_id, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z); -} - -/** - * @brief Encode a image_triggered struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param image_triggered C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_triggered_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_triggered_t* image_triggered) -{ - return mavlink_msg_image_triggered_pack_chan(system_id, component_id, chan, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z); -} - -/** - * @brief Send a image_triggered message - * @param chan MAVLink channel to send the message - * - * @param timestamp Timestamp - * @param seq IMU seq - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN]; - _mav_put_uint64_t(buf, 0, timestamp); - _mav_put_uint32_t(buf, 8, seq); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_float(buf, 24, local_z); - _mav_put_float(buf, 28, lat); - _mav_put_float(buf, 32, lon); - _mav_put_float(buf, 36, alt); - _mav_put_float(buf, 40, ground_x); - _mav_put_float(buf, 44, ground_y); - _mav_put_float(buf, 48, ground_z); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif -#else - mavlink_image_triggered_t packet; - packet.timestamp = timestamp; - packet.seq = seq; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.local_z = local_z; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.ground_x = ground_x; - packet.ground_y = ground_y; - packet.ground_z = ground_z; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)&packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)&packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_image_triggered_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, timestamp); - _mav_put_uint32_t(buf, 8, seq); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_float(buf, 24, local_z); - _mav_put_float(buf, 28, lat); - _mav_put_float(buf, 32, lon); - _mav_put_float(buf, 36, alt); - _mav_put_float(buf, 40, ground_x); - _mav_put_float(buf, 44, ground_y); - _mav_put_float(buf, 48, ground_z); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif -#else - mavlink_image_triggered_t *packet = (mavlink_image_triggered_t *)msgbuf; - packet->timestamp = timestamp; - packet->seq = seq; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - packet->local_z = local_z; - packet->lat = lat; - packet->lon = lon; - packet->alt = alt; - packet->ground_x = ground_x; - packet->ground_y = ground_y; - packet->ground_z = ground_z; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE IMAGE_TRIGGERED UNPACKING - - -/** - * @brief Get field timestamp from image_triggered message - * - * @return Timestamp - */ -static inline uint64_t mavlink_msg_image_triggered_get_timestamp(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field seq from image_triggered message - * - * @return IMU seq - */ -static inline uint32_t mavlink_msg_image_triggered_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 8); -} - -/** - * @brief Get field roll from image_triggered message - * - * @return Roll angle in rad - */ -static inline float mavlink_msg_image_triggered_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field pitch from image_triggered message - * - * @return Pitch angle in rad - */ -static inline float mavlink_msg_image_triggered_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field yaw from image_triggered message - * - * @return Yaw angle in rad - */ -static inline float mavlink_msg_image_triggered_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field local_z from image_triggered message - * - * @return Local frame Z coordinate (height over ground) - */ -static inline float mavlink_msg_image_triggered_get_local_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field lat from image_triggered message - * - * @return GPS X coordinate - */ -static inline float mavlink_msg_image_triggered_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field lon from image_triggered message - * - * @return GPS Y coordinate - */ -static inline float mavlink_msg_image_triggered_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Get field alt from image_triggered message - * - * @return Global frame altitude - */ -static inline float mavlink_msg_image_triggered_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 36); -} - -/** - * @brief Get field ground_x from image_triggered message - * - * @return Ground truth X - */ -static inline float mavlink_msg_image_triggered_get_ground_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 40); -} - -/** - * @brief Get field ground_y from image_triggered message - * - * @return Ground truth Y - */ -static inline float mavlink_msg_image_triggered_get_ground_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 44); -} - -/** - * @brief Get field ground_z from image_triggered message - * - * @return Ground truth Z - */ -static inline float mavlink_msg_image_triggered_get_ground_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 48); -} - -/** - * @brief Decode a image_triggered message into a struct - * - * @param msg The message to decode - * @param image_triggered C-struct to decode the message contents into - */ -static inline void mavlink_msg_image_triggered_decode(const mavlink_message_t* msg, mavlink_image_triggered_t* image_triggered) -{ -#if MAVLINK_NEED_BYTE_SWAP - image_triggered->timestamp = mavlink_msg_image_triggered_get_timestamp(msg); - image_triggered->seq = mavlink_msg_image_triggered_get_seq(msg); - image_triggered->roll = mavlink_msg_image_triggered_get_roll(msg); - image_triggered->pitch = mavlink_msg_image_triggered_get_pitch(msg); - image_triggered->yaw = mavlink_msg_image_triggered_get_yaw(msg); - image_triggered->local_z = mavlink_msg_image_triggered_get_local_z(msg); - image_triggered->lat = mavlink_msg_image_triggered_get_lat(msg); - image_triggered->lon = mavlink_msg_image_triggered_get_lon(msg); - image_triggered->alt = mavlink_msg_image_triggered_get_alt(msg); - image_triggered->ground_x = mavlink_msg_image_triggered_get_ground_x(msg); - image_triggered->ground_y = mavlink_msg_image_triggered_get_ground_y(msg); - image_triggered->ground_z = mavlink_msg_image_triggered_get_ground_z(msg); -#else - memcpy(image_triggered, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h deleted file mode 100644 index 05452637e..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE MARKER PACKING - -#define MAVLINK_MSG_ID_MARKER 171 - -typedef struct __mavlink_marker_t -{ - float x; ///< x position - float y; ///< y position - float z; ///< z position - float roll; ///< roll orientation - float pitch; ///< pitch orientation - float yaw; ///< yaw orientation - uint16_t id; ///< ID -} mavlink_marker_t; - -#define MAVLINK_MSG_ID_MARKER_LEN 26 -#define MAVLINK_MSG_ID_171_LEN 26 - -#define MAVLINK_MSG_ID_MARKER_CRC 249 -#define MAVLINK_MSG_ID_171_CRC 249 - - - -#define MAVLINK_MESSAGE_INFO_MARKER { \ - "MARKER", \ - 7, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_marker_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_marker_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_marker_t, z) }, \ - { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_marker_t, roll) }, \ - { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_marker_t, pitch) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_marker_t, yaw) }, \ - { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_marker_t, id) }, \ - } \ -} - - -/** - * @brief Pack a marker message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param id ID - * @param x x position - * @param y y position - * @param z z position - * @param roll roll orientation - * @param pitch pitch orientation - * @param yaw yaw orientation - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MARKER_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_uint16_t(buf, 24, id); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MARKER_LEN); -#else - mavlink_marker_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.id = id; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MARKER_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MARKER; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MARKER_LEN); -#endif -} - -/** - * @brief Pack a marker message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param id ID - * @param x x position - * @param y y position - * @param z z position - * @param roll roll orientation - * @param pitch pitch orientation - * @param yaw yaw orientation - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t id,float x,float y,float z,float roll,float pitch,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MARKER_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_uint16_t(buf, 24, id); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MARKER_LEN); -#else - mavlink_marker_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.id = id; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MARKER_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MARKER; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MARKER_LEN); -#endif -} - -/** - * @brief Encode a marker struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param marker C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_marker_t* marker) -{ - return mavlink_msg_marker_pack(system_id, component_id, msg, marker->id, marker->x, marker->y, marker->z, marker->roll, marker->pitch, marker->yaw); -} - -/** - * @brief Encode a marker struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param marker C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_marker_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_marker_t* marker) -{ - return mavlink_msg_marker_pack_chan(system_id, component_id, chan, msg, marker->id, marker->x, marker->y, marker->z, marker->roll, marker->pitch, marker->yaw); -} - -/** - * @brief Send a marker message - * @param chan MAVLink channel to send the message - * - * @param id ID - * @param x x position - * @param y y position - * @param z z position - * @param roll roll orientation - * @param pitch pitch orientation - * @param yaw yaw orientation - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MARKER_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_uint16_t(buf, 24, id); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, MAVLINK_MSG_ID_MARKER_LEN); -#endif -#else - mavlink_marker_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.roll = roll; - packet.pitch = pitch; - packet.yaw = yaw; - packet.id = id; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)&packet, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)&packet, MAVLINK_MSG_ID_MARKER_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_MARKER_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_marker_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, roll); - _mav_put_float(buf, 16, pitch); - _mav_put_float(buf, 20, yaw); - _mav_put_uint16_t(buf, 24, id); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, MAVLINK_MSG_ID_MARKER_LEN); -#endif -#else - mavlink_marker_t *packet = (mavlink_marker_t *)msgbuf; - packet->x = x; - packet->y = y; - packet->z = z; - packet->roll = roll; - packet->pitch = pitch; - packet->yaw = yaw; - packet->id = id; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)packet, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)packet, MAVLINK_MSG_ID_MARKER_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE MARKER UNPACKING - - -/** - * @brief Get field id from marker message - * - * @return ID - */ -static inline uint16_t mavlink_msg_marker_get_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 24); -} - -/** - * @brief Get field x from marker message - * - * @return x position - */ -static inline float mavlink_msg_marker_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from marker message - * - * @return y position - */ -static inline float mavlink_msg_marker_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from marker message - * - * @return z position - */ -static inline float mavlink_msg_marker_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field roll from marker message - * - * @return roll orientation - */ -static inline float mavlink_msg_marker_get_roll(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field pitch from marker message - * - * @return pitch orientation - */ -static inline float mavlink_msg_marker_get_pitch(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field yaw from marker message - * - * @return yaw orientation - */ -static inline float mavlink_msg_marker_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a marker message into a struct - * - * @param msg The message to decode - * @param marker C-struct to decode the message contents into - */ -static inline void mavlink_msg_marker_decode(const mavlink_message_t* msg, mavlink_marker_t* marker) -{ -#if MAVLINK_NEED_BYTE_SWAP - marker->x = mavlink_msg_marker_get_x(msg); - marker->y = mavlink_msg_marker_get_y(msg); - marker->z = mavlink_msg_marker_get_z(msg); - marker->roll = mavlink_msg_marker_get_roll(msg); - marker->pitch = mavlink_msg_marker_get_pitch(msg); - marker->yaw = mavlink_msg_marker_get_yaw(msg); - marker->id = mavlink_msg_marker_get_id(msg); -#else - memcpy(marker, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MARKER_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h deleted file mode 100644 index bde400da9..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h +++ /dev/null @@ -1,273 +0,0 @@ -// MESSAGE PATTERN_DETECTED PACKING - -#define MAVLINK_MSG_ID_PATTERN_DETECTED 190 - -typedef struct __mavlink_pattern_detected_t -{ - float confidence; ///< Confidence of detection - uint8_t type; ///< 0: Pattern, 1: Letter - char file[100]; ///< Pattern file name - uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes -} mavlink_pattern_detected_t; - -#define MAVLINK_MSG_ID_PATTERN_DETECTED_LEN 106 -#define MAVLINK_MSG_ID_190_LEN 106 - -#define MAVLINK_MSG_ID_PATTERN_DETECTED_CRC 90 -#define MAVLINK_MSG_ID_190_CRC 90 - -#define MAVLINK_MSG_PATTERN_DETECTED_FIELD_FILE_LEN 100 - -#define MAVLINK_MESSAGE_INFO_PATTERN_DETECTED { \ - "PATTERN_DETECTED", \ - 4, \ - { { "confidence", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_pattern_detected_t, confidence) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_pattern_detected_t, type) }, \ - { "file", NULL, MAVLINK_TYPE_CHAR, 100, 5, offsetof(mavlink_pattern_detected_t, file) }, \ - { "detected", NULL, MAVLINK_TYPE_UINT8_T, 0, 105, offsetof(mavlink_pattern_detected_t, detected) }, \ - } \ -} - - -/** - * @brief Pack a pattern_detected message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type 0: Pattern, 1: Letter - * @param confidence Confidence of detection - * @param file Pattern file name - * @param detected Accepted as true detection, 0 no, 1 yes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, float confidence, const char *file, uint8_t detected) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PATTERN_DETECTED_LEN]; - _mav_put_float(buf, 0, confidence); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 105, detected); - _mav_put_char_array(buf, 5, file, 100); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#else - mavlink_pattern_detected_t packet; - packet.confidence = confidence; - packet.type = type; - packet.detected = detected; - mav_array_memcpy(packet.file, file, sizeof(char)*100); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif -} - -/** - * @brief Pack a pattern_detected message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type 0: Pattern, 1: Letter - * @param confidence Confidence of detection - * @param file Pattern file name - * @param detected Accepted as true detection, 0 no, 1 yes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,float confidence,const char *file,uint8_t detected) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PATTERN_DETECTED_LEN]; - _mav_put_float(buf, 0, confidence); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 105, detected); - _mav_put_char_array(buf, 5, file, 100); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#else - mavlink_pattern_detected_t packet; - packet.confidence = confidence; - packet.type = type; - packet.detected = detected; - mav_array_memcpy(packet.file, file, sizeof(char)*100); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif -} - -/** - * @brief Encode a pattern_detected struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param pattern_detected C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_pattern_detected_t* pattern_detected) -{ - return mavlink_msg_pattern_detected_pack(system_id, component_id, msg, pattern_detected->type, pattern_detected->confidence, pattern_detected->file, pattern_detected->detected); -} - -/** - * @brief Encode a pattern_detected struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param pattern_detected C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_pattern_detected_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pattern_detected_t* pattern_detected) -{ - return mavlink_msg_pattern_detected_pack_chan(system_id, component_id, chan, msg, pattern_detected->type, pattern_detected->confidence, pattern_detected->file, pattern_detected->detected); -} - -/** - * @brief Send a pattern_detected message - * @param chan MAVLink channel to send the message - * - * @param type 0: Pattern, 1: Letter - * @param confidence Confidence of detection - * @param file Pattern file name - * @param detected Accepted as true detection, 0 no, 1 yes - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char *file, uint8_t detected) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PATTERN_DETECTED_LEN]; - _mav_put_float(buf, 0, confidence); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 105, detected); - _mav_put_char_array(buf, 5, file, 100); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif -#else - mavlink_pattern_detected_t packet; - packet.confidence = confidence; - packet.type = type; - packet.detected = detected; - mav_array_memcpy(packet.file, file, sizeof(char)*100); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)&packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)&packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_PATTERN_DETECTED_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_pattern_detected_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, float confidence, const char *file, uint8_t detected) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, confidence); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 105, detected); - _mav_put_char_array(buf, 5, file, 100); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif -#else - mavlink_pattern_detected_t *packet = (mavlink_pattern_detected_t *)msgbuf; - packet->confidence = confidence; - packet->type = type; - packet->detected = detected; - mav_array_memcpy(packet->file, file, sizeof(char)*100); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE PATTERN_DETECTED UNPACKING - - -/** - * @brief Get field type from pattern_detected message - * - * @return 0: Pattern, 1: Letter - */ -static inline uint8_t mavlink_msg_pattern_detected_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field confidence from pattern_detected message - * - * @return Confidence of detection - */ -static inline float mavlink_msg_pattern_detected_get_confidence(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field file from pattern_detected message - * - * @return Pattern file name - */ -static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_message_t* msg, char *file) -{ - return _MAV_RETURN_char_array(msg, file, 100, 5); -} - -/** - * @brief Get field detected from pattern_detected message - * - * @return Accepted as true detection, 0 no, 1 yes - */ -static inline uint8_t mavlink_msg_pattern_detected_get_detected(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 105); -} - -/** - * @brief Decode a pattern_detected message into a struct - * - * @param msg The message to decode - * @param pattern_detected C-struct to decode the message contents into - */ -static inline void mavlink_msg_pattern_detected_decode(const mavlink_message_t* msg, mavlink_pattern_detected_t* pattern_detected) -{ -#if MAVLINK_NEED_BYTE_SWAP - pattern_detected->confidence = mavlink_msg_pattern_detected_get_confidence(msg); - pattern_detected->type = mavlink_msg_pattern_detected_get_type(msg); - mavlink_msg_pattern_detected_get_file(msg, pattern_detected->file); - pattern_detected->detected = mavlink_msg_pattern_detected_get_detected(msg); -#else - memcpy(pattern_detected, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h deleted file mode 100644 index 86e934519..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h +++ /dev/null @@ -1,369 +0,0 @@ -// MESSAGE POINT_OF_INTEREST PACKING - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST 191 - -typedef struct __mavlink_point_of_interest_t -{ - float x; ///< X Position - float y; ///< Y Position - float z; ///< Z Position - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds - uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - uint8_t coordinate_system; ///< 0: global, 1:local - char name[26]; ///< POI name -} mavlink_point_of_interest_t; - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN 43 -#define MAVLINK_MSG_ID_191_LEN 43 - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC 95 -#define MAVLINK_MSG_ID_191_CRC 95 - -#define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 26 - -#define MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST { \ - "POINT_OF_INTEREST", \ - 8, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_point_of_interest_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_point_of_interest_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_point_of_interest_t, z) }, \ - { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_point_of_interest_t, timeout) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_point_of_interest_t, type) }, \ - { "color", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_point_of_interest_t, color) }, \ - { "coordinate_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_point_of_interest_t, coordinate_system) }, \ - { "name", NULL, MAVLINK_TYPE_CHAR, 26, 17, offsetof(mavlink_point_of_interest_t, name) }, \ - } \ -} - - -/** - * @brief Pack a point_of_interest message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param name POI name - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_uint16_t(buf, 12, timeout); - _mav_put_uint8_t(buf, 14, type); - _mav_put_uint8_t(buf, 15, color); - _mav_put_uint8_t(buf, 16, coordinate_system); - _mav_put_char_array(buf, 17, name, 26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#else - mavlink_point_of_interest_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.timeout = timeout; - packet.type = type; - packet.color = color; - packet.coordinate_system = coordinate_system; - mav_array_memcpy(packet.name, name, sizeof(char)*26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif -} - -/** - * @brief Pack a point_of_interest message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param name POI name - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t color,uint8_t coordinate_system,uint16_t timeout,float x,float y,float z,const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_uint16_t(buf, 12, timeout); - _mav_put_uint8_t(buf, 14, type); - _mav_put_uint8_t(buf, 15, color); - _mav_put_uint8_t(buf, 16, coordinate_system); - _mav_put_char_array(buf, 17, name, 26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#else - mavlink_point_of_interest_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.timeout = timeout; - packet.type = type; - packet.color = color; - packet.coordinate_system = coordinate_system; - mav_array_memcpy(packet.name, name, sizeof(char)*26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif -} - -/** - * @brief Encode a point_of_interest struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param point_of_interest C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_point_of_interest_t* point_of_interest) -{ - return mavlink_msg_point_of_interest_pack(system_id, component_id, msg, point_of_interest->type, point_of_interest->color, point_of_interest->coordinate_system, point_of_interest->timeout, point_of_interest->x, point_of_interest->y, point_of_interest->z, point_of_interest->name); -} - -/** - * @brief Encode a point_of_interest struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param point_of_interest C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_point_of_interest_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_point_of_interest_t* point_of_interest) -{ - return mavlink_msg_point_of_interest_pack_chan(system_id, component_id, chan, msg, point_of_interest->type, point_of_interest->color, point_of_interest->coordinate_system, point_of_interest->timeout, point_of_interest->x, point_of_interest->y, point_of_interest->z, point_of_interest->name); -} - -/** - * @brief Send a point_of_interest message - * @param chan MAVLink channel to send the message - * - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param name POI name - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_uint16_t(buf, 12, timeout); - _mav_put_uint8_t(buf, 14, type); - _mav_put_uint8_t(buf, 15, color); - _mav_put_uint8_t(buf, 16, coordinate_system); - _mav_put_char_array(buf, 17, name, 26); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif -#else - mavlink_point_of_interest_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.timeout = timeout; - packet.type = type; - packet.color = color; - packet.coordinate_system = coordinate_system; - mav_array_memcpy(packet.name, name, sizeof(char)*26); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)&packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)&packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_point_of_interest_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_uint16_t(buf, 12, timeout); - _mav_put_uint8_t(buf, 14, type); - _mav_put_uint8_t(buf, 15, color); - _mav_put_uint8_t(buf, 16, coordinate_system); - _mav_put_char_array(buf, 17, name, 26); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif -#else - mavlink_point_of_interest_t *packet = (mavlink_point_of_interest_t *)msgbuf; - packet->x = x; - packet->y = y; - packet->z = z; - packet->timeout = timeout; - packet->type = type; - packet->color = color; - packet->coordinate_system = coordinate_system; - mav_array_memcpy(packet->name, name, sizeof(char)*26); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE POINT_OF_INTEREST UNPACKING - - -/** - * @brief Get field type from point_of_interest message - * - * @return 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - */ -static inline uint8_t mavlink_msg_point_of_interest_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field color from point_of_interest message - * - * @return 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - */ -static inline uint8_t mavlink_msg_point_of_interest_get_color(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 15); -} - -/** - * @brief Get field coordinate_system from point_of_interest message - * - * @return 0: global, 1:local - */ -static inline uint8_t mavlink_msg_point_of_interest_get_coordinate_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field timeout from point_of_interest message - * - * @return 0: no timeout, >1: timeout in seconds - */ -static inline uint16_t mavlink_msg_point_of_interest_get_timeout(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field x from point_of_interest message - * - * @return X Position - */ -static inline float mavlink_msg_point_of_interest_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from point_of_interest message - * - * @return Y Position - */ -static inline float mavlink_msg_point_of_interest_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from point_of_interest message - * - * @return Z Position - */ -static inline float mavlink_msg_point_of_interest_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field name from point_of_interest message - * - * @return POI name - */ -static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_message_t* msg, char *name) -{ - return _MAV_RETURN_char_array(msg, name, 26, 17); -} - -/** - * @brief Decode a point_of_interest message into a struct - * - * @param msg The message to decode - * @param point_of_interest C-struct to decode the message contents into - */ -static inline void mavlink_msg_point_of_interest_decode(const mavlink_message_t* msg, mavlink_point_of_interest_t* point_of_interest) -{ -#if MAVLINK_NEED_BYTE_SWAP - point_of_interest->x = mavlink_msg_point_of_interest_get_x(msg); - point_of_interest->y = mavlink_msg_point_of_interest_get_y(msg); - point_of_interest->z = mavlink_msg_point_of_interest_get_z(msg); - point_of_interest->timeout = mavlink_msg_point_of_interest_get_timeout(msg); - point_of_interest->type = mavlink_msg_point_of_interest_get_type(msg); - point_of_interest->color = mavlink_msg_point_of_interest_get_color(msg); - point_of_interest->coordinate_system = mavlink_msg_point_of_interest_get_coordinate_system(msg); - mavlink_msg_point_of_interest_get_name(msg, point_of_interest->name); -#else - memcpy(point_of_interest, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h deleted file mode 100644 index e5c04a851..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h +++ /dev/null @@ -1,441 +0,0 @@ -// MESSAGE POINT_OF_INTEREST_CONNECTION PACKING - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION 192 - -typedef struct __mavlink_point_of_interest_connection_t -{ - float xp1; ///< X1 Position - float yp1; ///< Y1 Position - float zp1; ///< Z1 Position - float xp2; ///< X2 Position - float yp2; ///< Y2 Position - float zp2; ///< Z2 Position - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds - uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - uint8_t coordinate_system; ///< 0: global, 1:local - char name[26]; ///< POI connection name -} mavlink_point_of_interest_connection_t; - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN 55 -#define MAVLINK_MSG_ID_192_LEN 55 - -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC 36 -#define MAVLINK_MSG_ID_192_CRC 36 - -#define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 26 - -#define MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION { \ - "POINT_OF_INTEREST_CONNECTION", \ - 11, \ - { { "xp1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_point_of_interest_connection_t, xp1) }, \ - { "yp1", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_point_of_interest_connection_t, yp1) }, \ - { "zp1", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_point_of_interest_connection_t, zp1) }, \ - { "xp2", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_point_of_interest_connection_t, xp2) }, \ - { "yp2", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_point_of_interest_connection_t, yp2) }, \ - { "zp2", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_point_of_interest_connection_t, zp2) }, \ - { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_point_of_interest_connection_t, timeout) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_point_of_interest_connection_t, type) }, \ - { "color", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_point_of_interest_connection_t, color) }, \ - { "coordinate_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_point_of_interest_connection_t, coordinate_system) }, \ - { "name", NULL, MAVLINK_TYPE_CHAR, 26, 29, offsetof(mavlink_point_of_interest_connection_t, name) }, \ - } \ -} - - -/** - * @brief Pack a point_of_interest_connection message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param xp1 X1 Position - * @param yp1 Y1 Position - * @param zp1 Z1 Position - * @param xp2 X2 Position - * @param yp2 Y2 Position - * @param zp2 Z2 Position - * @param name POI connection name - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN]; - _mav_put_float(buf, 0, xp1); - _mav_put_float(buf, 4, yp1); - _mav_put_float(buf, 8, zp1); - _mav_put_float(buf, 12, xp2); - _mav_put_float(buf, 16, yp2); - _mav_put_float(buf, 20, zp2); - _mav_put_uint16_t(buf, 24, timeout); - _mav_put_uint8_t(buf, 26, type); - _mav_put_uint8_t(buf, 27, color); - _mav_put_uint8_t(buf, 28, coordinate_system); - _mav_put_char_array(buf, 29, name, 26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#else - mavlink_point_of_interest_connection_t packet; - packet.xp1 = xp1; - packet.yp1 = yp1; - packet.zp1 = zp1; - packet.xp2 = xp2; - packet.yp2 = yp2; - packet.zp2 = zp2; - packet.timeout = timeout; - packet.type = type; - packet.color = color; - packet.coordinate_system = coordinate_system; - mav_array_memcpy(packet.name, name, sizeof(char)*26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif -} - -/** - * @brief Pack a point_of_interest_connection message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param xp1 X1 Position - * @param yp1 Y1 Position - * @param zp1 Z1 Position - * @param xp2 X2 Position - * @param yp2 Y2 Position - * @param zp2 Z2 Position - * @param name POI connection name - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t color,uint8_t coordinate_system,uint16_t timeout,float xp1,float yp1,float zp1,float xp2,float yp2,float zp2,const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN]; - _mav_put_float(buf, 0, xp1); - _mav_put_float(buf, 4, yp1); - _mav_put_float(buf, 8, zp1); - _mav_put_float(buf, 12, xp2); - _mav_put_float(buf, 16, yp2); - _mav_put_float(buf, 20, zp2); - _mav_put_uint16_t(buf, 24, timeout); - _mav_put_uint8_t(buf, 26, type); - _mav_put_uint8_t(buf, 27, color); - _mav_put_uint8_t(buf, 28, coordinate_system); - _mav_put_char_array(buf, 29, name, 26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#else - mavlink_point_of_interest_connection_t packet; - packet.xp1 = xp1; - packet.yp1 = yp1; - packet.zp1 = zp1; - packet.xp2 = xp2; - packet.yp2 = yp2; - packet.zp2 = zp2; - packet.timeout = timeout; - packet.type = type; - packet.color = color; - packet.coordinate_system = coordinate_system; - mav_array_memcpy(packet.name, name, sizeof(char)*26); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif -} - -/** - * @brief Encode a point_of_interest_connection struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param point_of_interest_connection C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_point_of_interest_connection_t* point_of_interest_connection) -{ - return mavlink_msg_point_of_interest_connection_pack(system_id, component_id, msg, point_of_interest_connection->type, point_of_interest_connection->color, point_of_interest_connection->coordinate_system, point_of_interest_connection->timeout, point_of_interest_connection->xp1, point_of_interest_connection->yp1, point_of_interest_connection->zp1, point_of_interest_connection->xp2, point_of_interest_connection->yp2, point_of_interest_connection->zp2, point_of_interest_connection->name); -} - -/** - * @brief Encode a point_of_interest_connection struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param point_of_interest_connection C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_point_of_interest_connection_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_point_of_interest_connection_t* point_of_interest_connection) -{ - return mavlink_msg_point_of_interest_connection_pack_chan(system_id, component_id, chan, msg, point_of_interest_connection->type, point_of_interest_connection->color, point_of_interest_connection->coordinate_system, point_of_interest_connection->timeout, point_of_interest_connection->xp1, point_of_interest_connection->yp1, point_of_interest_connection->zp1, point_of_interest_connection->xp2, point_of_interest_connection->yp2, point_of_interest_connection->zp2, point_of_interest_connection->name); -} - -/** - * @brief Send a point_of_interest_connection message - * @param chan MAVLink channel to send the message - * - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param xp1 X1 Position - * @param yp1 Y1 Position - * @param zp1 Z1 Position - * @param xp2 X2 Position - * @param yp2 Y2 Position - * @param zp2 Z2 Position - * @param name POI connection name - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN]; - _mav_put_float(buf, 0, xp1); - _mav_put_float(buf, 4, yp1); - _mav_put_float(buf, 8, zp1); - _mav_put_float(buf, 12, xp2); - _mav_put_float(buf, 16, yp2); - _mav_put_float(buf, 20, zp2); - _mav_put_uint16_t(buf, 24, timeout); - _mav_put_uint8_t(buf, 26, type); - _mav_put_uint8_t(buf, 27, color); - _mav_put_uint8_t(buf, 28, coordinate_system); - _mav_put_char_array(buf, 29, name, 26); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif -#else - mavlink_point_of_interest_connection_t packet; - packet.xp1 = xp1; - packet.yp1 = yp1; - packet.zp1 = zp1; - packet.xp2 = xp2; - packet.yp2 = yp2; - packet.zp2 = zp2; - packet.timeout = timeout; - packet.type = type; - packet.color = color; - packet.coordinate_system = coordinate_system; - mav_array_memcpy(packet.name, name, sizeof(char)*26); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)&packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)&packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_point_of_interest_connection_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char *name) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, xp1); - _mav_put_float(buf, 4, yp1); - _mav_put_float(buf, 8, zp1); - _mav_put_float(buf, 12, xp2); - _mav_put_float(buf, 16, yp2); - _mav_put_float(buf, 20, zp2); - _mav_put_uint16_t(buf, 24, timeout); - _mav_put_uint8_t(buf, 26, type); - _mav_put_uint8_t(buf, 27, color); - _mav_put_uint8_t(buf, 28, coordinate_system); - _mav_put_char_array(buf, 29, name, 26); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif -#else - mavlink_point_of_interest_connection_t *packet = (mavlink_point_of_interest_connection_t *)msgbuf; - packet->xp1 = xp1; - packet->yp1 = yp1; - packet->zp1 = zp1; - packet->xp2 = xp2; - packet->yp2 = yp2; - packet->zp2 = zp2; - packet->timeout = timeout; - packet->type = type; - packet->color = color; - packet->coordinate_system = coordinate_system; - mav_array_memcpy(packet->name, name, sizeof(char)*26); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE POINT_OF_INTEREST_CONNECTION UNPACKING - - -/** - * @brief Get field type from point_of_interest_connection message - * - * @return 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - */ -static inline uint8_t mavlink_msg_point_of_interest_connection_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 26); -} - -/** - * @brief Get field color from point_of_interest_connection message - * - * @return 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - */ -static inline uint8_t mavlink_msg_point_of_interest_connection_get_color(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 27); -} - -/** - * @brief Get field coordinate_system from point_of_interest_connection message - * - * @return 0: global, 1:local - */ -static inline uint8_t mavlink_msg_point_of_interest_connection_get_coordinate_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 28); -} - -/** - * @brief Get field timeout from point_of_interest_connection message - * - * @return 0: no timeout, >1: timeout in seconds - */ -static inline uint16_t mavlink_msg_point_of_interest_connection_get_timeout(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 24); -} - -/** - * @brief Get field xp1 from point_of_interest_connection message - * - * @return X1 Position - */ -static inline float mavlink_msg_point_of_interest_connection_get_xp1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field yp1 from point_of_interest_connection message - * - * @return Y1 Position - */ -static inline float mavlink_msg_point_of_interest_connection_get_yp1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field zp1 from point_of_interest_connection message - * - * @return Z1 Position - */ -static inline float mavlink_msg_point_of_interest_connection_get_zp1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field xp2 from point_of_interest_connection message - * - * @return X2 Position - */ -static inline float mavlink_msg_point_of_interest_connection_get_xp2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field yp2 from point_of_interest_connection message - * - * @return Y2 Position - */ -static inline float mavlink_msg_point_of_interest_connection_get_yp2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field zp2 from point_of_interest_connection message - * - * @return Z2 Position - */ -static inline float mavlink_msg_point_of_interest_connection_get_zp2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field name from point_of_interest_connection message - * - * @return POI connection name - */ -static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const mavlink_message_t* msg, char *name) -{ - return _MAV_RETURN_char_array(msg, name, 26, 29); -} - -/** - * @brief Decode a point_of_interest_connection message into a struct - * - * @param msg The message to decode - * @param point_of_interest_connection C-struct to decode the message contents into - */ -static inline void mavlink_msg_point_of_interest_connection_decode(const mavlink_message_t* msg, mavlink_point_of_interest_connection_t* point_of_interest_connection) -{ -#if MAVLINK_NEED_BYTE_SWAP - point_of_interest_connection->xp1 = mavlink_msg_point_of_interest_connection_get_xp1(msg); - point_of_interest_connection->yp1 = mavlink_msg_point_of_interest_connection_get_yp1(msg); - point_of_interest_connection->zp1 = mavlink_msg_point_of_interest_connection_get_zp1(msg); - point_of_interest_connection->xp2 = mavlink_msg_point_of_interest_connection_get_xp2(msg); - point_of_interest_connection->yp2 = mavlink_msg_point_of_interest_connection_get_yp2(msg); - point_of_interest_connection->zp2 = mavlink_msg_point_of_interest_connection_get_zp2(msg); - point_of_interest_connection->timeout = mavlink_msg_point_of_interest_connection_get_timeout(msg); - point_of_interest_connection->type = mavlink_msg_point_of_interest_connection_get_type(msg); - point_of_interest_connection->color = mavlink_msg_point_of_interest_connection_get_color(msg); - point_of_interest_connection->coordinate_system = mavlink_msg_point_of_interest_connection_get_coordinate_system(msg); - mavlink_msg_point_of_interest_connection_get_name(msg, point_of_interest_connection->name); -#else - memcpy(point_of_interest_connection, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h deleted file mode 100644 index 480004cef..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h +++ /dev/null @@ -1,305 +0,0 @@ -// MESSAGE POSITION_CONTROL_SETPOINT PACKING - -#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT 170 - -typedef struct __mavlink_position_control_setpoint_t -{ - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< yaw orientation in radians, 0 = NORTH - uint16_t id; ///< ID of waypoint, 0 for plain position -} mavlink_position_control_setpoint_t; - -#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN 18 -#define MAVLINK_MSG_ID_170_LEN 18 - -#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC 28 -#define MAVLINK_MSG_ID_170_CRC 28 - - - -#define MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT { \ - "POSITION_CONTROL_SETPOINT", \ - 5, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_control_setpoint_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_control_setpoint_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_control_setpoint_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_control_setpoint_t, yaw) }, \ - { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_position_control_setpoint_t, id) }, \ - } \ -} - - -/** - * @brief Pack a position_control_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param id ID of waypoint, 0 for plain position - * @param x x position - * @param y y position - * @param z z position - * @param yaw yaw orientation in radians, 0 = NORTH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t id, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint16_t(buf, 16, id); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#else - mavlink_position_control_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.id = id; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif -} - -/** - * @brief Pack a position_control_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param id ID of waypoint, 0 for plain position - * @param x x position - * @param y y position - * @param z z position - * @param yaw yaw orientation in radians, 0 = NORTH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t id,float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint16_t(buf, 16, id); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#else - mavlink_position_control_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.id = id; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif -} - -/** - * @brief Encode a position_control_setpoint struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param position_control_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_control_setpoint_t* position_control_setpoint) -{ - return mavlink_msg_position_control_setpoint_pack(system_id, component_id, msg, position_control_setpoint->id, position_control_setpoint->x, position_control_setpoint->y, position_control_setpoint->z, position_control_setpoint->yaw); -} - -/** - * @brief Encode a position_control_setpoint struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param position_control_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_position_control_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_position_control_setpoint_t* position_control_setpoint) -{ - return mavlink_msg_position_control_setpoint_pack_chan(system_id, component_id, chan, msg, position_control_setpoint->id, position_control_setpoint->x, position_control_setpoint->y, position_control_setpoint->z, position_control_setpoint->yaw); -} - -/** - * @brief Send a position_control_setpoint message - * @param chan MAVLink channel to send the message - * - * @param id ID of waypoint, 0 for plain position - * @param x x position - * @param y y position - * @param z z position - * @param yaw yaw orientation in radians, 0 = NORTH - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint16_t(buf, 16, id); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif -#else - mavlink_position_control_setpoint_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.id = id; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)&packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_position_control_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint16_t(buf, 16, id); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif -#else - mavlink_position_control_setpoint_t *packet = (mavlink_position_control_setpoint_t *)msgbuf; - packet->x = x; - packet->y = y; - packet->z = z; - packet->yaw = yaw; - packet->id = id; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE POSITION_CONTROL_SETPOINT UNPACKING - - -/** - * @brief Get field id from position_control_setpoint message - * - * @return ID of waypoint, 0 for plain position - */ -static inline uint16_t mavlink_msg_position_control_setpoint_get_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 16); -} - -/** - * @brief Get field x from position_control_setpoint message - * - * @return x position - */ -static inline float mavlink_msg_position_control_setpoint_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from position_control_setpoint message - * - * @return y position - */ -static inline float mavlink_msg_position_control_setpoint_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from position_control_setpoint message - * - * @return z position - */ -static inline float mavlink_msg_position_control_setpoint_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from position_control_setpoint message - * - * @return yaw orientation in radians, 0 = NORTH - */ -static inline float mavlink_msg_position_control_setpoint_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a position_control_setpoint message into a struct - * - * @param msg The message to decode - * @param position_control_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_position_control_setpoint_decode(const mavlink_message_t* msg, mavlink_position_control_setpoint_t* position_control_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - position_control_setpoint->x = mavlink_msg_position_control_setpoint_get_x(msg); - position_control_setpoint->y = mavlink_msg_position_control_setpoint_get_y(msg); - position_control_setpoint->z = mavlink_msg_position_control_setpoint_get_z(msg); - position_control_setpoint->yaw = mavlink_msg_position_control_setpoint_get_yaw(msg); - position_control_setpoint->id = mavlink_msg_position_control_setpoint_get_id(msg); -#else - memcpy(position_control_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h deleted file mode 100644 index 41a512648..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h +++ /dev/null @@ -1,353 +0,0 @@ -// MESSAGE RAW_AUX PACKING - -#define MAVLINK_MSG_ID_RAW_AUX 172 - -typedef struct __mavlink_raw_aux_t -{ - int32_t baro; ///< Barometric pressure (hecto Pascal) - uint16_t adc1; ///< ADC1 (J405 ADC3, LPC2148 AD0.6) - uint16_t adc2; ///< ADC2 (J405 ADC5, LPC2148 AD0.2) - uint16_t adc3; ///< ADC3 (J405 ADC6, LPC2148 AD0.1) - uint16_t adc4; ///< ADC4 (J405 ADC7, LPC2148 AD1.3) - uint16_t vbat; ///< Battery voltage - int16_t temp; ///< Temperature (degrees celcius) -} mavlink_raw_aux_t; - -#define MAVLINK_MSG_ID_RAW_AUX_LEN 16 -#define MAVLINK_MSG_ID_172_LEN 16 - -#define MAVLINK_MSG_ID_RAW_AUX_CRC 182 -#define MAVLINK_MSG_ID_172_CRC 182 - - - -#define MAVLINK_MESSAGE_INFO_RAW_AUX { \ - "RAW_AUX", \ - 7, \ - { { "baro", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_raw_aux_t, baro) }, \ - { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_raw_aux_t, adc1) }, \ - { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_raw_aux_t, adc2) }, \ - { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_raw_aux_t, adc3) }, \ - { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_raw_aux_t, adc4) }, \ - { "vbat", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_raw_aux_t, vbat) }, \ - { "temp", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_aux_t, temp) }, \ - } \ -} - - -/** - * @brief Pack a raw_aux message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param adc1 ADC1 (J405 ADC3, LPC2148 AD0.6) - * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2) - * @param adc3 ADC3 (J405 ADC6, LPC2148 AD0.1) - * @param adc4 ADC4 (J405 ADC7, LPC2148 AD1.3) - * @param vbat Battery voltage - * @param temp Temperature (degrees celcius) - * @param baro Barometric pressure (hecto Pascal) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RAW_AUX_LEN]; - _mav_put_int32_t(buf, 0, baro); - _mav_put_uint16_t(buf, 4, adc1); - _mav_put_uint16_t(buf, 6, adc2); - _mav_put_uint16_t(buf, 8, adc3); - _mav_put_uint16_t(buf, 10, adc4); - _mav_put_uint16_t(buf, 12, vbat); - _mav_put_int16_t(buf, 14, temp); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_AUX_LEN); -#else - mavlink_raw_aux_t packet; - packet.baro = baro; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.vbat = vbat; - packet.temp = temp; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RAW_AUX; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif -} - -/** - * @brief Pack a raw_aux message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param adc1 ADC1 (J405 ADC3, LPC2148 AD0.6) - * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2) - * @param adc3 ADC3 (J405 ADC6, LPC2148 AD0.1) - * @param adc4 ADC4 (J405 ADC7, LPC2148 AD1.3) - * @param vbat Battery voltage - * @param temp Temperature (degrees celcius) - * @param baro Barometric pressure (hecto Pascal) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t vbat,int16_t temp,int32_t baro) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RAW_AUX_LEN]; - _mav_put_int32_t(buf, 0, baro); - _mav_put_uint16_t(buf, 4, adc1); - _mav_put_uint16_t(buf, 6, adc2); - _mav_put_uint16_t(buf, 8, adc3); - _mav_put_uint16_t(buf, 10, adc4); - _mav_put_uint16_t(buf, 12, vbat); - _mav_put_int16_t(buf, 14, temp); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_AUX_LEN); -#else - mavlink_raw_aux_t packet; - packet.baro = baro; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.vbat = vbat; - packet.temp = temp; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_RAW_AUX; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif -} - -/** - * @brief Encode a raw_aux struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param raw_aux C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_aux_t* raw_aux) -{ - return mavlink_msg_raw_aux_pack(system_id, component_id, msg, raw_aux->adc1, raw_aux->adc2, raw_aux->adc3, raw_aux->adc4, raw_aux->vbat, raw_aux->temp, raw_aux->baro); -} - -/** - * @brief Encode a raw_aux struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param raw_aux C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_raw_aux_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_aux_t* raw_aux) -{ - return mavlink_msg_raw_aux_pack_chan(system_id, component_id, chan, msg, raw_aux->adc1, raw_aux->adc2, raw_aux->adc3, raw_aux->adc4, raw_aux->vbat, raw_aux->temp, raw_aux->baro); -} - -/** - * @brief Send a raw_aux message - * @param chan MAVLink channel to send the message - * - * @param adc1 ADC1 (J405 ADC3, LPC2148 AD0.6) - * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2) - * @param adc3 ADC3 (J405 ADC6, LPC2148 AD0.1) - * @param adc4 ADC4 (J405 ADC7, LPC2148 AD1.3) - * @param vbat Battery voltage - * @param temp Temperature (degrees celcius) - * @param baro Barometric pressure (hecto Pascal) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RAW_AUX_LEN]; - _mav_put_int32_t(buf, 0, baro); - _mav_put_uint16_t(buf, 4, adc1); - _mav_put_uint16_t(buf, 6, adc2); - _mav_put_uint16_t(buf, 8, adc3); - _mav_put_uint16_t(buf, 10, adc4); - _mav_put_uint16_t(buf, 12, vbat); - _mav_put_int16_t(buf, 14, temp); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif -#else - mavlink_raw_aux_t packet; - packet.baro = baro; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.vbat = vbat; - packet.temp = temp; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)&packet, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)&packet, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_RAW_AUX_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_raw_aux_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, baro); - _mav_put_uint16_t(buf, 4, adc1); - _mav_put_uint16_t(buf, 6, adc2); - _mav_put_uint16_t(buf, 8, adc3); - _mav_put_uint16_t(buf, 10, adc4); - _mav_put_uint16_t(buf, 12, vbat); - _mav_put_int16_t(buf, 14, temp); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif -#else - mavlink_raw_aux_t *packet = (mavlink_raw_aux_t *)msgbuf; - packet->baro = baro; - packet->adc1 = adc1; - packet->adc2 = adc2; - packet->adc3 = adc3; - packet->adc4 = adc4; - packet->vbat = vbat; - packet->temp = temp; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)packet, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)packet, MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE RAW_AUX UNPACKING - - -/** - * @brief Get field adc1 from raw_aux message - * - * @return ADC1 (J405 ADC3, LPC2148 AD0.6) - */ -static inline uint16_t mavlink_msg_raw_aux_get_adc1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field adc2 from raw_aux message - * - * @return ADC2 (J405 ADC5, LPC2148 AD0.2) - */ -static inline uint16_t mavlink_msg_raw_aux_get_adc2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field adc3 from raw_aux message - * - * @return ADC3 (J405 ADC6, LPC2148 AD0.1) - */ -static inline uint16_t mavlink_msg_raw_aux_get_adc3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field adc4 from raw_aux message - * - * @return ADC4 (J405 ADC7, LPC2148 AD1.3) - */ -static inline uint16_t mavlink_msg_raw_aux_get_adc4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Get field vbat from raw_aux message - * - * @return Battery voltage - */ -static inline uint16_t mavlink_msg_raw_aux_get_vbat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field temp from raw_aux message - * - * @return Temperature (degrees celcius) - */ -static inline int16_t mavlink_msg_raw_aux_get_temp(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 14); -} - -/** - * @brief Get field baro from raw_aux message - * - * @return Barometric pressure (hecto Pascal) - */ -static inline int32_t mavlink_msg_raw_aux_get_baro(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Decode a raw_aux message into a struct - * - * @param msg The message to decode - * @param raw_aux C-struct to decode the message contents into - */ -static inline void mavlink_msg_raw_aux_decode(const mavlink_message_t* msg, mavlink_raw_aux_t* raw_aux) -{ -#if MAVLINK_NEED_BYTE_SWAP - raw_aux->baro = mavlink_msg_raw_aux_get_baro(msg); - raw_aux->adc1 = mavlink_msg_raw_aux_get_adc1(msg); - raw_aux->adc2 = mavlink_msg_raw_aux_get_adc2(msg); - raw_aux->adc3 = mavlink_msg_raw_aux_get_adc3(msg); - raw_aux->adc4 = mavlink_msg_raw_aux_get_adc4(msg); - raw_aux->vbat = mavlink_msg_raw_aux_get_vbat(msg); - raw_aux->temp = mavlink_msg_raw_aux_get_temp(msg); -#else - memcpy(raw_aux, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RAW_AUX_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h deleted file mode 100644 index 8fe14dd54..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE SET_CAM_SHUTTER PACKING - -#define MAVLINK_MSG_ID_SET_CAM_SHUTTER 151 - -typedef struct __mavlink_set_cam_shutter_t -{ - float gain; ///< Camera gain - uint16_t interval; ///< Shutter interval, in microseconds - uint16_t exposure; ///< Exposure time, in microseconds - uint8_t cam_no; ///< Camera id - uint8_t cam_mode; ///< Camera mode: 0 = auto, 1 = manual - uint8_t trigger_pin; ///< Trigger pin, 0-3 for PtGrey FireFly -} mavlink_set_cam_shutter_t; - -#define MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN 11 -#define MAVLINK_MSG_ID_151_LEN 11 - -#define MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC 108 -#define MAVLINK_MSG_ID_151_CRC 108 - - - -#define MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER { \ - "SET_CAM_SHUTTER", \ - 6, \ - { { "gain", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_cam_shutter_t, gain) }, \ - { "interval", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_set_cam_shutter_t, interval) }, \ - { "exposure", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_set_cam_shutter_t, exposure) }, \ - { "cam_no", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_set_cam_shutter_t, cam_no) }, \ - { "cam_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_set_cam_shutter_t, cam_mode) }, \ - { "trigger_pin", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_set_cam_shutter_t, trigger_pin) }, \ - } \ -} - - -/** - * @brief Pack a set_cam_shutter message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param cam_no Camera id - * @param cam_mode Camera mode: 0 = auto, 1 = manual - * @param trigger_pin Trigger pin, 0-3 for PtGrey FireFly - * @param interval Shutter interval, in microseconds - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN]; - _mav_put_float(buf, 0, gain); - _mav_put_uint16_t(buf, 4, interval); - _mav_put_uint16_t(buf, 6, exposure); - _mav_put_uint8_t(buf, 8, cam_no); - _mav_put_uint8_t(buf, 9, cam_mode); - _mav_put_uint8_t(buf, 10, trigger_pin); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#else - mavlink_set_cam_shutter_t packet; - packet.gain = gain; - packet.interval = interval; - packet.exposure = exposure; - packet.cam_no = cam_no; - packet.cam_mode = cam_mode; - packet.trigger_pin = trigger_pin; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif -} - -/** - * @brief Pack a set_cam_shutter message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param cam_no Camera id - * @param cam_mode Camera mode: 0 = auto, 1 = manual - * @param trigger_pin Trigger pin, 0-3 for PtGrey FireFly - * @param interval Shutter interval, in microseconds - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t cam_no,uint8_t cam_mode,uint8_t trigger_pin,uint16_t interval,uint16_t exposure,float gain) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN]; - _mav_put_float(buf, 0, gain); - _mav_put_uint16_t(buf, 4, interval); - _mav_put_uint16_t(buf, 6, exposure); - _mav_put_uint8_t(buf, 8, cam_no); - _mav_put_uint8_t(buf, 9, cam_mode); - _mav_put_uint8_t(buf, 10, trigger_pin); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#else - mavlink_set_cam_shutter_t packet; - packet.gain = gain; - packet.interval = interval; - packet.exposure = exposure; - packet.cam_no = cam_no; - packet.cam_mode = cam_mode; - packet.trigger_pin = trigger_pin; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif -} - -/** - * @brief Encode a set_cam_shutter struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_cam_shutter C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_cam_shutter_t* set_cam_shutter) -{ - return mavlink_msg_set_cam_shutter_pack(system_id, component_id, msg, set_cam_shutter->cam_no, set_cam_shutter->cam_mode, set_cam_shutter->trigger_pin, set_cam_shutter->interval, set_cam_shutter->exposure, set_cam_shutter->gain); -} - -/** - * @brief Encode a set_cam_shutter struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_cam_shutter C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_cam_shutter_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_cam_shutter_t* set_cam_shutter) -{ - return mavlink_msg_set_cam_shutter_pack_chan(system_id, component_id, chan, msg, set_cam_shutter->cam_no, set_cam_shutter->cam_mode, set_cam_shutter->trigger_pin, set_cam_shutter->interval, set_cam_shutter->exposure, set_cam_shutter->gain); -} - -/** - * @brief Send a set_cam_shutter message - * @param chan MAVLink channel to send the message - * - * @param cam_no Camera id - * @param cam_mode Camera mode: 0 = auto, 1 = manual - * @param trigger_pin Trigger pin, 0-3 for PtGrey FireFly - * @param interval Shutter interval, in microseconds - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN]; - _mav_put_float(buf, 0, gain); - _mav_put_uint16_t(buf, 4, interval); - _mav_put_uint16_t(buf, 6, exposure); - _mav_put_uint8_t(buf, 8, cam_no); - _mav_put_uint8_t(buf, 9, cam_mode); - _mav_put_uint8_t(buf, 10, trigger_pin); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif -#else - mavlink_set_cam_shutter_t packet; - packet.gain = gain; - packet.interval = interval; - packet.exposure = exposure; - packet.cam_no = cam_no; - packet.cam_mode = cam_mode; - packet.trigger_pin = trigger_pin; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)&packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)&packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_set_cam_shutter_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, gain); - _mav_put_uint16_t(buf, 4, interval); - _mav_put_uint16_t(buf, 6, exposure); - _mav_put_uint8_t(buf, 8, cam_no); - _mav_put_uint8_t(buf, 9, cam_mode); - _mav_put_uint8_t(buf, 10, trigger_pin); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif -#else - mavlink_set_cam_shutter_t *packet = (mavlink_set_cam_shutter_t *)msgbuf; - packet->gain = gain; - packet->interval = interval; - packet->exposure = exposure; - packet->cam_no = cam_no; - packet->cam_mode = cam_mode; - packet->trigger_pin = trigger_pin; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SET_CAM_SHUTTER UNPACKING - - -/** - * @brief Get field cam_no from set_cam_shutter message - * - * @return Camera id - */ -static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_no(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 8); -} - -/** - * @brief Get field cam_mode from set_cam_shutter message - * - * @return Camera mode: 0 = auto, 1 = manual - */ -static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 9); -} - -/** - * @brief Get field trigger_pin from set_cam_shutter message - * - * @return Trigger pin, 0-3 for PtGrey FireFly - */ -static inline uint8_t mavlink_msg_set_cam_shutter_get_trigger_pin(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field interval from set_cam_shutter message - * - * @return Shutter interval, in microseconds - */ -static inline uint16_t mavlink_msg_set_cam_shutter_get_interval(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field exposure from set_cam_shutter message - * - * @return Exposure time, in microseconds - */ -static inline uint16_t mavlink_msg_set_cam_shutter_get_exposure(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field gain from set_cam_shutter message - * - * @return Camera gain - */ -static inline float mavlink_msg_set_cam_shutter_get_gain(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a set_cam_shutter message into a struct - * - * @param msg The message to decode - * @param set_cam_shutter C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_cam_shutter_decode(const mavlink_message_t* msg, mavlink_set_cam_shutter_t* set_cam_shutter) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_cam_shutter->gain = mavlink_msg_set_cam_shutter_get_gain(msg); - set_cam_shutter->interval = mavlink_msg_set_cam_shutter_get_interval(msg); - set_cam_shutter->exposure = mavlink_msg_set_cam_shutter_get_exposure(msg); - set_cam_shutter->cam_no = mavlink_msg_set_cam_shutter_get_cam_no(msg); - set_cam_shutter->cam_mode = mavlink_msg_set_cam_shutter_get_cam_mode(msg); - set_cam_shutter->trigger_pin = mavlink_msg_set_cam_shutter_get_trigger_pin(msg); -#else - memcpy(set_cam_shutter, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h deleted file mode 100644 index c4e4fc19a..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE SET_POSITION_CONTROL_OFFSET PACKING - -#define MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET 160 - -typedef struct __mavlink_set_position_control_offset_t -{ - float x; ///< x position offset - float y; ///< y position offset - float z; ///< z position offset - float yaw; ///< yaw orientation offset in radians, 0 = NORTH - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_set_position_control_offset_t; - -#define MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN 18 -#define MAVLINK_MSG_ID_160_LEN 18 - -#define MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC 22 -#define MAVLINK_MSG_ID_160_CRC 22 - - - -#define MAVLINK_MESSAGE_INFO_SET_POSITION_CONTROL_OFFSET { \ - "SET_POSITION_CONTROL_OFFSET", \ - 6, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_position_control_offset_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_position_control_offset_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_position_control_offset_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_position_control_offset_t, yaw) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_position_control_offset_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_position_control_offset_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a set_position_control_offset message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param target_component Component ID - * @param x x position offset - * @param y y position offset - * @param z z position offset - * @param yaw yaw orientation offset in radians, 0 = NORTH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_position_control_offset_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#else - mavlink_set_position_control_offset_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif -} - -/** - * @brief Pack a set_position_control_offset message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param x x position offset - * @param y y position offset - * @param z z position offset - * @param yaw yaw orientation offset in radians, 0 = NORTH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_position_control_offset_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#else - mavlink_set_position_control_offset_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif -} - -/** - * @brief Encode a set_position_control_offset struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param set_position_control_offset C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_position_control_offset_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_position_control_offset_t* set_position_control_offset) -{ - return mavlink_msg_set_position_control_offset_pack(system_id, component_id, msg, set_position_control_offset->target_system, set_position_control_offset->target_component, set_position_control_offset->x, set_position_control_offset->y, set_position_control_offset->z, set_position_control_offset->yaw); -} - -/** - * @brief Encode a set_position_control_offset struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param set_position_control_offset C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_position_control_offset_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_position_control_offset_t* set_position_control_offset) -{ - return mavlink_msg_set_position_control_offset_pack_chan(system_id, component_id, chan, msg, set_position_control_offset->target_system, set_position_control_offset->target_component, set_position_control_offset->x, set_position_control_offset->y, set_position_control_offset->z, set_position_control_offset->yaw); -} - -/** - * @brief Send a set_position_control_offset message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param x x position offset - * @param y y position offset - * @param z z position offset - * @param yaw yaw orientation offset in radians, 0 = NORTH - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_position_control_offset_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif -#else - mavlink_set_position_control_offset_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)&packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)&packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_set_position_control_offset_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif -#else - mavlink_set_position_control_offset_t *packet = (mavlink_set_position_control_offset_t *)msgbuf; - packet->x = x; - packet->y = y; - packet->z = z; - packet->yaw = yaw; - packet->target_system = target_system; - packet->target_component = target_component; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SET_POSITION_CONTROL_OFFSET UNPACKING - - -/** - * @brief Get field target_system from set_position_control_offset message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_set_position_control_offset_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from set_position_control_offset message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_set_position_control_offset_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field x from set_position_control_offset message - * - * @return x position offset - */ -static inline float mavlink_msg_set_position_control_offset_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from set_position_control_offset message - * - * @return y position offset - */ -static inline float mavlink_msg_set_position_control_offset_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from set_position_control_offset message - * - * @return z position offset - */ -static inline float mavlink_msg_set_position_control_offset_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from set_position_control_offset message - * - * @return yaw orientation offset in radians, 0 = NORTH - */ -static inline float mavlink_msg_set_position_control_offset_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a set_position_control_offset message into a struct - * - * @param msg The message to decode - * @param set_position_control_offset C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_position_control_offset_decode(const mavlink_message_t* msg, mavlink_set_position_control_offset_t* set_position_control_offset) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_position_control_offset->x = mavlink_msg_set_position_control_offset_get_x(msg); - set_position_control_offset->y = mavlink_msg_set_position_control_offset_get_y(msg); - set_position_control_offset->z = mavlink_msg_set_position_control_offset_get_z(msg); - set_position_control_offset->yaw = mavlink_msg_set_position_control_offset_get_yaw(msg); - set_position_control_offset->target_system = mavlink_msg_set_position_control_offset_get_target_system(msg); - set_position_control_offset->target_component = mavlink_msg_set_position_control_offset_get_target_component(msg); -#else - memcpy(set_position_control_offset, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h deleted file mode 100644 index d71a2ed2e..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h +++ /dev/null @@ -1,281 +0,0 @@ -// MESSAGE WATCHDOG_COMMAND PACKING - -#define MAVLINK_MSG_ID_WATCHDOG_COMMAND 183 - -typedef struct __mavlink_watchdog_command_t -{ - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - uint8_t target_system_id; ///< Target system ID - uint8_t command_id; ///< Command ID -} mavlink_watchdog_command_t; - -#define MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN 6 -#define MAVLINK_MSG_ID_183_LEN 6 - -#define MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC 162 -#define MAVLINK_MSG_ID_183_CRC 162 - - - -#define MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND { \ - "WATCHDOG_COMMAND", \ - 4, \ - { { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_watchdog_command_t, watchdog_id) }, \ - { "process_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_watchdog_command_t, process_id) }, \ - { "target_system_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_watchdog_command_t, target_system_id) }, \ - { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_watchdog_command_t, command_id) }, \ - } \ -} - - -/** - * @brief Pack a watchdog_command message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system_id Target system ID - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param command_id Command ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_id); - _mav_put_uint8_t(buf, 4, target_system_id); - _mav_put_uint8_t(buf, 5, command_id); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#else - mavlink_watchdog_command_t packet; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - packet.target_system_id = target_system_id; - packet.command_id = command_id; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif -} - -/** - * @brief Pack a watchdog_command message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system_id Target system ID - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param command_id Command ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system_id,uint16_t watchdog_id,uint16_t process_id,uint8_t command_id) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_id); - _mav_put_uint8_t(buf, 4, target_system_id); - _mav_put_uint8_t(buf, 5, command_id); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#else - mavlink_watchdog_command_t packet; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - packet.target_system_id = target_system_id; - packet.command_id = command_id; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif -} - -/** - * @brief Encode a watchdog_command struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param watchdog_command C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_command_t* watchdog_command) -{ - return mavlink_msg_watchdog_command_pack(system_id, component_id, msg, watchdog_command->target_system_id, watchdog_command->watchdog_id, watchdog_command->process_id, watchdog_command->command_id); -} - -/** - * @brief Encode a watchdog_command struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_command C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_command_t* watchdog_command) -{ - return mavlink_msg_watchdog_command_pack_chan(system_id, component_id, chan, msg, watchdog_command->target_system_id, watchdog_command->watchdog_id, watchdog_command->process_id, watchdog_command->command_id); -} - -/** - * @brief Send a watchdog_command message - * @param chan MAVLink channel to send the message - * - * @param target_system_id Target system ID - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param command_id Command ID - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_id); - _mav_put_uint8_t(buf, 4, target_system_id); - _mav_put_uint8_t(buf, 5, command_id); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif -#else - mavlink_watchdog_command_t packet; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - packet.target_system_id = target_system_id; - packet.command_id = command_id; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_watchdog_command_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_id); - _mav_put_uint8_t(buf, 4, target_system_id); - _mav_put_uint8_t(buf, 5, command_id); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif -#else - mavlink_watchdog_command_t *packet = (mavlink_watchdog_command_t *)msgbuf; - packet->watchdog_id = watchdog_id; - packet->process_id = process_id; - packet->target_system_id = target_system_id; - packet->command_id = command_id; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE WATCHDOG_COMMAND UNPACKING - - -/** - * @brief Get field target_system_id from watchdog_command message - * - * @return Target system ID - */ -static inline uint8_t mavlink_msg_watchdog_command_get_target_system_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field watchdog_id from watchdog_command message - * - * @return Watchdog ID - */ -static inline uint16_t mavlink_msg_watchdog_command_get_watchdog_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field process_id from watchdog_command message - * - * @return Process ID - */ -static inline uint16_t mavlink_msg_watchdog_command_get_process_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field command_id from watchdog_command message - * - * @return Command ID - */ -static inline uint8_t mavlink_msg_watchdog_command_get_command_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Decode a watchdog_command message into a struct - * - * @param msg The message to decode - * @param watchdog_command C-struct to decode the message contents into - */ -static inline void mavlink_msg_watchdog_command_decode(const mavlink_message_t* msg, mavlink_watchdog_command_t* watchdog_command) -{ -#if MAVLINK_NEED_BYTE_SWAP - watchdog_command->watchdog_id = mavlink_msg_watchdog_command_get_watchdog_id(msg); - watchdog_command->process_id = mavlink_msg_watchdog_command_get_process_id(msg); - watchdog_command->target_system_id = mavlink_msg_watchdog_command_get_target_system_id(msg); - watchdog_command->command_id = mavlink_msg_watchdog_command_get_command_id(msg); -#else - memcpy(watchdog_command, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h deleted file mode 100644 index a8e49b642..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ /dev/null @@ -1,233 +0,0 @@ -// MESSAGE WATCHDOG_HEARTBEAT PACKING - -#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT 180 - -typedef struct __mavlink_watchdog_heartbeat_t -{ - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_count; ///< Number of processes -} mavlink_watchdog_heartbeat_t; - -#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN 4 -#define MAVLINK_MSG_ID_180_LEN 4 - -#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC 153 -#define MAVLINK_MSG_ID_180_CRC 153 - - - -#define MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT { \ - "WATCHDOG_HEARTBEAT", \ - 2, \ - { { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_watchdog_heartbeat_t, watchdog_id) }, \ - { "process_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_watchdog_heartbeat_t, process_count) }, \ - } \ -} - - -/** - * @brief Pack a watchdog_heartbeat message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param watchdog_id Watchdog ID - * @param process_count Number of processes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t watchdog_id, uint16_t process_count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_count); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#else - mavlink_watchdog_heartbeat_t packet; - packet.watchdog_id = watchdog_id; - packet.process_count = process_count; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif -} - -/** - * @brief Pack a watchdog_heartbeat message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_id Watchdog ID - * @param process_count Number of processes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t watchdog_id,uint16_t process_count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_count); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#else - mavlink_watchdog_heartbeat_t packet; - packet.watchdog_id = watchdog_id; - packet.process_count = process_count; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif -} - -/** - * @brief Encode a watchdog_heartbeat struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param watchdog_heartbeat C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_heartbeat_t* watchdog_heartbeat) -{ - return mavlink_msg_watchdog_heartbeat_pack(system_id, component_id, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count); -} - -/** - * @brief Encode a watchdog_heartbeat struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_heartbeat C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_heartbeat_t* watchdog_heartbeat) -{ - return mavlink_msg_watchdog_heartbeat_pack_chan(system_id, component_id, chan, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count); -} - -/** - * @brief Send a watchdog_heartbeat message - * @param chan MAVLink channel to send the message - * - * @param watchdog_id Watchdog ID - * @param process_count Number of processes - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN]; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_count); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif -#else - mavlink_watchdog_heartbeat_t packet; - packet.watchdog_id = watchdog_id; - packet.process_count = process_count; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_watchdog_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, watchdog_id); - _mav_put_uint16_t(buf, 2, process_count); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif -#else - mavlink_watchdog_heartbeat_t *packet = (mavlink_watchdog_heartbeat_t *)msgbuf; - packet->watchdog_id = watchdog_id; - packet->process_count = process_count; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE WATCHDOG_HEARTBEAT UNPACKING - - -/** - * @brief Get field watchdog_id from watchdog_heartbeat message - * - * @return Watchdog ID - */ -static inline uint16_t mavlink_msg_watchdog_heartbeat_get_watchdog_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field process_count from watchdog_heartbeat message - * - * @return Number of processes - */ -static inline uint16_t mavlink_msg_watchdog_heartbeat_get_process_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Decode a watchdog_heartbeat message into a struct - * - * @param msg The message to decode - * @param watchdog_heartbeat C-struct to decode the message contents into - */ -static inline void mavlink_msg_watchdog_heartbeat_decode(const mavlink_message_t* msg, mavlink_watchdog_heartbeat_t* watchdog_heartbeat) -{ -#if MAVLINK_NEED_BYTE_SWAP - watchdog_heartbeat->watchdog_id = mavlink_msg_watchdog_heartbeat_get_watchdog_id(msg); - watchdog_heartbeat->process_count = mavlink_msg_watchdog_heartbeat_get_process_count(msg); -#else - memcpy(watchdog_heartbeat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h deleted file mode 100644 index a5e0cc67e..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h +++ /dev/null @@ -1,298 +0,0 @@ -// MESSAGE WATCHDOG_PROCESS_INFO PACKING - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO 181 - -typedef struct __mavlink_watchdog_process_info_t -{ - int32_t timeout; ///< Timeout (seconds) - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - char name[100]; ///< Process name - char arguments[147]; ///< Process arguments -} mavlink_watchdog_process_info_t; - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN 255 -#define MAVLINK_MSG_ID_181_LEN 255 - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC 16 -#define MAVLINK_MSG_ID_181_CRC 16 - -#define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100 -#define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_ARGUMENTS_LEN 147 - -#define MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO { \ - "WATCHDOG_PROCESS_INFO", \ - 5, \ - { { "timeout", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_watchdog_process_info_t, timeout) }, \ - { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_watchdog_process_info_t, watchdog_id) }, \ - { "process_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_watchdog_process_info_t, process_id) }, \ - { "name", NULL, MAVLINK_TYPE_CHAR, 100, 8, offsetof(mavlink_watchdog_process_info_t, name) }, \ - { "arguments", NULL, MAVLINK_TYPE_CHAR, 147, 108, offsetof(mavlink_watchdog_process_info_t, arguments) }, \ - } \ -} - - -/** - * @brief Pack a watchdog_process_info message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param name Process name - * @param arguments Process arguments - * @param timeout Timeout (seconds) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t watchdog_id, uint16_t process_id, const char *name, const char *arguments, int32_t timeout) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN]; - _mav_put_int32_t(buf, 0, timeout); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_char_array(buf, 8, name, 100); - _mav_put_char_array(buf, 108, arguments, 147); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#else - mavlink_watchdog_process_info_t packet; - packet.timeout = timeout; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - mav_array_memcpy(packet.name, name, sizeof(char)*100); - mav_array_memcpy(packet.arguments, arguments, sizeof(char)*147); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif -} - -/** - * @brief Pack a watchdog_process_info message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param name Process name - * @param arguments Process arguments - * @param timeout Timeout (seconds) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t watchdog_id,uint16_t process_id,const char *name,const char *arguments,int32_t timeout) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN]; - _mav_put_int32_t(buf, 0, timeout); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_char_array(buf, 8, name, 100); - _mav_put_char_array(buf, 108, arguments, 147); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#else - mavlink_watchdog_process_info_t packet; - packet.timeout = timeout; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - mav_array_memcpy(packet.name, name, sizeof(char)*100); - mav_array_memcpy(packet.arguments, arguments, sizeof(char)*147); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif -} - -/** - * @brief Encode a watchdog_process_info struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param watchdog_process_info C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_process_info_t* watchdog_process_info) -{ - return mavlink_msg_watchdog_process_info_pack(system_id, component_id, msg, watchdog_process_info->watchdog_id, watchdog_process_info->process_id, watchdog_process_info->name, watchdog_process_info->arguments, watchdog_process_info->timeout); -} - -/** - * @brief Encode a watchdog_process_info struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_process_info C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_process_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_process_info_t* watchdog_process_info) -{ - return mavlink_msg_watchdog_process_info_pack_chan(system_id, component_id, chan, msg, watchdog_process_info->watchdog_id, watchdog_process_info->process_id, watchdog_process_info->name, watchdog_process_info->arguments, watchdog_process_info->timeout); -} - -/** - * @brief Send a watchdog_process_info message - * @param chan MAVLink channel to send the message - * - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param name Process name - * @param arguments Process arguments - * @param timeout Timeout (seconds) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char *name, const char *arguments, int32_t timeout) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN]; - _mav_put_int32_t(buf, 0, timeout); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_char_array(buf, 8, name, 100); - _mav_put_char_array(buf, 108, arguments, 147); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif -#else - mavlink_watchdog_process_info_t packet; - packet.timeout = timeout; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - mav_array_memcpy(packet.name, name, sizeof(char)*100); - mav_array_memcpy(packet.arguments, arguments, sizeof(char)*147); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_watchdog_process_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char *name, const char *arguments, int32_t timeout) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, timeout); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_char_array(buf, 8, name, 100); - _mav_put_char_array(buf, 108, arguments, 147); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif -#else - mavlink_watchdog_process_info_t *packet = (mavlink_watchdog_process_info_t *)msgbuf; - packet->timeout = timeout; - packet->watchdog_id = watchdog_id; - packet->process_id = process_id; - mav_array_memcpy(packet->name, name, sizeof(char)*100); - mav_array_memcpy(packet->arguments, arguments, sizeof(char)*147); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE WATCHDOG_PROCESS_INFO UNPACKING - - -/** - * @brief Get field watchdog_id from watchdog_process_info message - * - * @return Watchdog ID - */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_watchdog_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field process_id from watchdog_process_info message - * - * @return Process ID - */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_process_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field name from watchdog_process_info message - * - * @return Process name - */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_message_t* msg, char *name) -{ - return _MAV_RETURN_char_array(msg, name, 100, 8); -} - -/** - * @brief Get field arguments from watchdog_process_info message - * - * @return Process arguments - */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mavlink_message_t* msg, char *arguments) -{ - return _MAV_RETURN_char_array(msg, arguments, 147, 108); -} - -/** - * @brief Get field timeout from watchdog_process_info message - * - * @return Timeout (seconds) - */ -static inline int32_t mavlink_msg_watchdog_process_info_get_timeout(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Decode a watchdog_process_info message into a struct - * - * @param msg The message to decode - * @param watchdog_process_info C-struct to decode the message contents into - */ -static inline void mavlink_msg_watchdog_process_info_decode(const mavlink_message_t* msg, mavlink_watchdog_process_info_t* watchdog_process_info) -{ -#if MAVLINK_NEED_BYTE_SWAP - watchdog_process_info->timeout = mavlink_msg_watchdog_process_info_get_timeout(msg); - watchdog_process_info->watchdog_id = mavlink_msg_watchdog_process_info_get_watchdog_id(msg); - watchdog_process_info->process_id = mavlink_msg_watchdog_process_info_get_process_id(msg); - mavlink_msg_watchdog_process_info_get_name(msg, watchdog_process_info->name); - mavlink_msg_watchdog_process_info_get_arguments(msg, watchdog_process_info->arguments); -#else - memcpy(watchdog_process_info, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h deleted file mode 100644 index 15c9598e4..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h +++ /dev/null @@ -1,329 +0,0 @@ -// MESSAGE WATCHDOG_PROCESS_STATUS PACKING - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS 182 - -typedef struct __mavlink_watchdog_process_status_t -{ - int32_t pid; ///< PID - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - uint16_t crashes; ///< Number of crashes - uint8_t state; ///< Is running / finished / suspended / crashed - uint8_t muted; ///< Is muted -} mavlink_watchdog_process_status_t; - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN 12 -#define MAVLINK_MSG_ID_182_LEN 12 - -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC 29 -#define MAVLINK_MSG_ID_182_CRC 29 - - - -#define MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS { \ - "WATCHDOG_PROCESS_STATUS", \ - 6, \ - { { "pid", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_watchdog_process_status_t, pid) }, \ - { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_watchdog_process_status_t, watchdog_id) }, \ - { "process_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_watchdog_process_status_t, process_id) }, \ - { "crashes", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_watchdog_process_status_t, crashes) }, \ - { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_watchdog_process_status_t, state) }, \ - { "muted", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_watchdog_process_status_t, muted) }, \ - } \ -} - - -/** - * @brief Pack a watchdog_process_status message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param state Is running / finished / suspended / crashed - * @param muted Is muted - * @param pid PID - * @param crashes Number of crashes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN]; - _mav_put_int32_t(buf, 0, pid); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_uint16_t(buf, 8, crashes); - _mav_put_uint8_t(buf, 10, state); - _mav_put_uint8_t(buf, 11, muted); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#else - mavlink_watchdog_process_status_t packet; - packet.pid = pid; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - packet.crashes = crashes; - packet.state = state; - packet.muted = muted; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif -} - -/** - * @brief Pack a watchdog_process_status message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param state Is running / finished / suspended / crashed - * @param muted Is muted - * @param pid PID - * @param crashes Number of crashes - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t watchdog_id,uint16_t process_id,uint8_t state,uint8_t muted,int32_t pid,uint16_t crashes) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN]; - _mav_put_int32_t(buf, 0, pid); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_uint16_t(buf, 8, crashes); - _mav_put_uint8_t(buf, 10, state); - _mav_put_uint8_t(buf, 11, muted); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#else - mavlink_watchdog_process_status_t packet; - packet.pid = pid; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - packet.crashes = crashes; - packet.state = state; - packet.muted = muted; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif -} - -/** - * @brief Encode a watchdog_process_status struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param watchdog_process_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_process_status_t* watchdog_process_status) -{ - return mavlink_msg_watchdog_process_status_pack(system_id, component_id, msg, watchdog_process_status->watchdog_id, watchdog_process_status->process_id, watchdog_process_status->state, watchdog_process_status->muted, watchdog_process_status->pid, watchdog_process_status->crashes); -} - -/** - * @brief Encode a watchdog_process_status struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_process_status C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_watchdog_process_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_process_status_t* watchdog_process_status) -{ - return mavlink_msg_watchdog_process_status_pack_chan(system_id, component_id, chan, msg, watchdog_process_status->watchdog_id, watchdog_process_status->process_id, watchdog_process_status->state, watchdog_process_status->muted, watchdog_process_status->pid, watchdog_process_status->crashes); -} - -/** - * @brief Send a watchdog_process_status message - * @param chan MAVLink channel to send the message - * - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param state Is running / finished / suspended / crashed - * @param muted Is muted - * @param pid PID - * @param crashes Number of crashes - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN]; - _mav_put_int32_t(buf, 0, pid); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_uint16_t(buf, 8, crashes); - _mav_put_uint8_t(buf, 10, state); - _mav_put_uint8_t(buf, 11, muted); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif -#else - mavlink_watchdog_process_status_t packet; - packet.pid = pid; - packet.watchdog_id = watchdog_id; - packet.process_id = process_id; - packet.crashes = crashes; - packet.state = state; - packet.muted = muted; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_watchdog_process_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, pid); - _mav_put_uint16_t(buf, 4, watchdog_id); - _mav_put_uint16_t(buf, 6, process_id); - _mav_put_uint16_t(buf, 8, crashes); - _mav_put_uint8_t(buf, 10, state); - _mav_put_uint8_t(buf, 11, muted); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif -#else - mavlink_watchdog_process_status_t *packet = (mavlink_watchdog_process_status_t *)msgbuf; - packet->pid = pid; - packet->watchdog_id = watchdog_id; - packet->process_id = process_id; - packet->crashes = crashes; - packet->state = state; - packet->muted = muted; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE WATCHDOG_PROCESS_STATUS UNPACKING - - -/** - * @brief Get field watchdog_id from watchdog_process_status message - * - * @return Watchdog ID - */ -static inline uint16_t mavlink_msg_watchdog_process_status_get_watchdog_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field process_id from watchdog_process_status message - * - * @return Process ID - */ -static inline uint16_t mavlink_msg_watchdog_process_status_get_process_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field state from watchdog_process_status message - * - * @return Is running / finished / suspended / crashed - */ -static inline uint8_t mavlink_msg_watchdog_process_status_get_state(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 10); -} - -/** - * @brief Get field muted from watchdog_process_status message - * - * @return Is muted - */ -static inline uint8_t mavlink_msg_watchdog_process_status_get_muted(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field pid from watchdog_process_status message - * - * @return PID - */ -static inline int32_t mavlink_msg_watchdog_process_status_get_pid(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field crashes from watchdog_process_status message - * - * @return Number of crashes - */ -static inline uint16_t mavlink_msg_watchdog_process_status_get_crashes(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Decode a watchdog_process_status message into a struct - * - * @param msg The message to decode - * @param watchdog_process_status C-struct to decode the message contents into - */ -static inline void mavlink_msg_watchdog_process_status_decode(const mavlink_message_t* msg, mavlink_watchdog_process_status_t* watchdog_process_status) -{ -#if MAVLINK_NEED_BYTE_SWAP - watchdog_process_status->pid = mavlink_msg_watchdog_process_status_get_pid(msg); - watchdog_process_status->watchdog_id = mavlink_msg_watchdog_process_status_get_watchdog_id(msg); - watchdog_process_status->process_id = mavlink_msg_watchdog_process_status_get_process_id(msg); - watchdog_process_status->crashes = mavlink_msg_watchdog_process_status_get_crashes(msg); - watchdog_process_status->state = mavlink_msg_watchdog_process_status_get_state(msg); - watchdog_process_status->muted = mavlink_msg_watchdog_process_status_get_muted(msg); -#else - memcpy(watchdog_process_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h deleted file mode 100644 index a624a2579..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h +++ /dev/null @@ -1,136 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from pixhawk.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef PIXHAWK_H -#define PIXHAWK_H - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 0, 0, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 108, 86, 95, 224, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 0, 0, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_POSITION_CONTROL_OFFSET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_PIXHAWK - -// ENUM DEFINITIONS - - -/** @brief Content Types for data transmission handshake */ -#ifndef HAVE_ENUM_DATA_TYPES -#define HAVE_ENUM_DATA_TYPES -enum DATA_TYPES -{ - DATA_TYPE_JPEG_IMAGE=1, /* | */ - DATA_TYPE_RAW_IMAGE=2, /* | */ - DATA_TYPE_KINECT=3, /* | */ - DATA_TYPES_ENUM_END=4, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_CMD -#define HAVE_ENUM_MAV_CMD -enum MAV_CMD -{ - MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ - MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ - MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ - MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_ENUM_END=501, /* | */ - MAV_CMD_DO_START_SEARCH=10001, /* Starts a search |1 to arm, 0 to disarm| */ - MAV_CMD_DO_FINISH_SEARCH=10002, /* Starts a search |1 to arm, 0 to disarm| */ - MAV_CMD_NAV_SWEEP=10003, /* Starts a search |1 to arm, 0 to disarm| */ -}; -#endif - -#include "../common/common.h" - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_set_cam_shutter.h" -#include "./mavlink_msg_image_triggered.h" -#include "./mavlink_msg_image_trigger_control.h" -#include "./mavlink_msg_image_available.h" -#include "./mavlink_msg_set_position_control_offset.h" -#include "./mavlink_msg_position_control_setpoint.h" -#include "./mavlink_msg_marker.h" -#include "./mavlink_msg_raw_aux.h" -#include "./mavlink_msg_watchdog_heartbeat.h" -#include "./mavlink_msg_watchdog_process_info.h" -#include "./mavlink_msg_watchdog_process_status.h" -#include "./mavlink_msg_watchdog_command.h" -#include "./mavlink_msg_pattern_detected.h" -#include "./mavlink_msg_point_of_interest.h" -#include "./mavlink_msg_point_of_interest_connection.h" -#include "./mavlink_msg_brief_feature.h" -#include "./mavlink_msg_attitude_control.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // PIXHAWK_H diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h deleted file mode 100644 index 7556606e9..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h +++ /dev/null @@ -1,3663 +0,0 @@ -// Generated by the protocol buffer compiler. DO NOT EDIT! -// source: pixhawk.proto - -#ifndef PROTOBUF_pixhawk_2eproto__INCLUDED -#define PROTOBUF_pixhawk_2eproto__INCLUDED - -#include - -#include - -#if GOOGLE_PROTOBUF_VERSION < 2004000 -#error This file was generated by a newer version of protoc which is -#error incompatible with your Protocol Buffer headers. Please update -#error your headers. -#endif -#if 2004001 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION -#error This file was generated by an older version of protoc which is -#error incompatible with your Protocol Buffer headers. Please -#error regenerate this file with a newer version of protoc. -#endif - -#include -#include -#include -#include -// @@protoc_insertion_point(includes) - -namespace px { - -// Internal implementation detail -- do not call these. -void protobuf_AddDesc_pixhawk_2eproto(); -void protobuf_AssignDesc_pixhawk_2eproto(); -void protobuf_ShutdownFile_pixhawk_2eproto(); - -class HeaderInfo; -class GLOverlay; -class Obstacle; -class ObstacleList; -class ObstacleMap; -class Path; -class PointCloudXYZI; -class PointCloudXYZI_PointXYZI; -class PointCloudXYZRGB; -class PointCloudXYZRGB_PointXYZRGB; -class RGBDImage; -class Waypoint; - -enum GLOverlay_CoordinateFrameType { - GLOverlay_CoordinateFrameType_GLOBAL = 0, - GLOverlay_CoordinateFrameType_LOCAL = 1 -}; -bool GLOverlay_CoordinateFrameType_IsValid(int value); -const GLOverlay_CoordinateFrameType GLOverlay_CoordinateFrameType_CoordinateFrameType_MIN = GLOverlay_CoordinateFrameType_GLOBAL; -const GLOverlay_CoordinateFrameType GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX = GLOverlay_CoordinateFrameType_LOCAL; -const int GLOverlay_CoordinateFrameType_CoordinateFrameType_ARRAYSIZE = GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX + 1; - -const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor(); -inline const ::std::string& GLOverlay_CoordinateFrameType_Name(GLOverlay_CoordinateFrameType value) { - return ::google::protobuf::internal::NameOfEnum( - GLOverlay_CoordinateFrameType_descriptor(), value); -} -inline bool GLOverlay_CoordinateFrameType_Parse( - const ::std::string& name, GLOverlay_CoordinateFrameType* value) { - return ::google::protobuf::internal::ParseNamedEnum( - GLOverlay_CoordinateFrameType_descriptor(), name, value); -} -enum GLOverlay_Mode { - GLOverlay_Mode_POINTS = 0, - GLOverlay_Mode_LINES = 1, - GLOverlay_Mode_LINE_STRIP = 2, - GLOverlay_Mode_LINE_LOOP = 3, - GLOverlay_Mode_TRIANGLES = 4, - GLOverlay_Mode_TRIANGLE_STRIP = 5, - GLOverlay_Mode_TRIANGLE_FAN = 6, - GLOverlay_Mode_QUADS = 7, - GLOverlay_Mode_QUAD_STRIP = 8, - GLOverlay_Mode_POLYGON = 9, - GLOverlay_Mode_SOLID_CIRCLE = 10, - GLOverlay_Mode_WIRE_CIRCLE = 11, - GLOverlay_Mode_SOLID_CUBE = 12, - GLOverlay_Mode_WIRE_CUBE = 13 -}; -bool GLOverlay_Mode_IsValid(int value); -const GLOverlay_Mode GLOverlay_Mode_Mode_MIN = GLOverlay_Mode_POINTS; -const GLOverlay_Mode GLOverlay_Mode_Mode_MAX = GLOverlay_Mode_WIRE_CUBE; -const int GLOverlay_Mode_Mode_ARRAYSIZE = GLOverlay_Mode_Mode_MAX + 1; - -const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor(); -inline const ::std::string& GLOverlay_Mode_Name(GLOverlay_Mode value) { - return ::google::protobuf::internal::NameOfEnum( - GLOverlay_Mode_descriptor(), value); -} -inline bool GLOverlay_Mode_Parse( - const ::std::string& name, GLOverlay_Mode* value) { - return ::google::protobuf::internal::ParseNamedEnum( - GLOverlay_Mode_descriptor(), name, value); -} -enum GLOverlay_Identifier { - GLOverlay_Identifier_END = 14, - GLOverlay_Identifier_VERTEX2F = 15, - GLOverlay_Identifier_VERTEX3F = 16, - GLOverlay_Identifier_ROTATEF = 17, - GLOverlay_Identifier_TRANSLATEF = 18, - GLOverlay_Identifier_SCALEF = 19, - GLOverlay_Identifier_PUSH_MATRIX = 20, - GLOverlay_Identifier_POP_MATRIX = 21, - GLOverlay_Identifier_COLOR3F = 22, - GLOverlay_Identifier_COLOR4F = 23, - GLOverlay_Identifier_POINTSIZE = 24, - GLOverlay_Identifier_LINEWIDTH = 25 -}; -bool GLOverlay_Identifier_IsValid(int value); -const GLOverlay_Identifier GLOverlay_Identifier_Identifier_MIN = GLOverlay_Identifier_END; -const GLOverlay_Identifier GLOverlay_Identifier_Identifier_MAX = GLOverlay_Identifier_LINEWIDTH; -const int GLOverlay_Identifier_Identifier_ARRAYSIZE = GLOverlay_Identifier_Identifier_MAX + 1; - -const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor(); -inline const ::std::string& GLOverlay_Identifier_Name(GLOverlay_Identifier value) { - return ::google::protobuf::internal::NameOfEnum( - GLOverlay_Identifier_descriptor(), value); -} -inline bool GLOverlay_Identifier_Parse( - const ::std::string& name, GLOverlay_Identifier* value) { - return ::google::protobuf::internal::ParseNamedEnum( - GLOverlay_Identifier_descriptor(), name, value); -} -// =================================================================== - -class HeaderInfo : public ::google::protobuf::Message { - public: - HeaderInfo(); - virtual ~HeaderInfo(); - - HeaderInfo(const HeaderInfo& from); - - inline HeaderInfo& operator=(const HeaderInfo& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const HeaderInfo& default_instance(); - - void Swap(HeaderInfo* other); - - // implements Message ---------------------------------------------- - - HeaderInfo* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const HeaderInfo& from); - void MergeFrom(const HeaderInfo& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required int32 source_sysid = 1; - inline bool has_source_sysid() const; - inline void clear_source_sysid(); - static const int kSourceSysidFieldNumber = 1; - inline ::google::protobuf::int32 source_sysid() const; - inline void set_source_sysid(::google::protobuf::int32 value); - - // required int32 source_compid = 2; - inline bool has_source_compid() const; - inline void clear_source_compid(); - static const int kSourceCompidFieldNumber = 2; - inline ::google::protobuf::int32 source_compid() const; - inline void set_source_compid(::google::protobuf::int32 value); - - // required double timestamp = 3; - inline bool has_timestamp() const; - inline void clear_timestamp(); - static const int kTimestampFieldNumber = 3; - inline double timestamp() const; - inline void set_timestamp(double value); - - // @@protoc_insertion_point(class_scope:px.HeaderInfo) - private: - inline void set_has_source_sysid(); - inline void clear_has_source_sysid(); - inline void set_has_source_compid(); - inline void clear_has_source_compid(); - inline void set_has_timestamp(); - inline void clear_has_timestamp(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::google::protobuf::int32 source_sysid_; - ::google::protobuf::int32 source_compid_; - double timestamp_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(3 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static HeaderInfo* default_instance_; -}; -// ------------------------------------------------------------------- - -class GLOverlay : public ::google::protobuf::Message { - public: - GLOverlay(); - virtual ~GLOverlay(); - - GLOverlay(const GLOverlay& from); - - inline GLOverlay& operator=(const GLOverlay& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const GLOverlay& default_instance(); - - void Swap(GLOverlay* other); - - // implements Message ---------------------------------------------- - - GLOverlay* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const GLOverlay& from); - void MergeFrom(const GLOverlay& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - typedef GLOverlay_CoordinateFrameType CoordinateFrameType; - static const CoordinateFrameType GLOBAL = GLOverlay_CoordinateFrameType_GLOBAL; - static const CoordinateFrameType LOCAL = GLOverlay_CoordinateFrameType_LOCAL; - static inline bool CoordinateFrameType_IsValid(int value) { - return GLOverlay_CoordinateFrameType_IsValid(value); - } - static const CoordinateFrameType CoordinateFrameType_MIN = - GLOverlay_CoordinateFrameType_CoordinateFrameType_MIN; - static const CoordinateFrameType CoordinateFrameType_MAX = - GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX; - static const int CoordinateFrameType_ARRAYSIZE = - GLOverlay_CoordinateFrameType_CoordinateFrameType_ARRAYSIZE; - static inline const ::google::protobuf::EnumDescriptor* - CoordinateFrameType_descriptor() { - return GLOverlay_CoordinateFrameType_descriptor(); - } - static inline const ::std::string& CoordinateFrameType_Name(CoordinateFrameType value) { - return GLOverlay_CoordinateFrameType_Name(value); - } - static inline bool CoordinateFrameType_Parse(const ::std::string& name, - CoordinateFrameType* value) { - return GLOverlay_CoordinateFrameType_Parse(name, value); - } - - typedef GLOverlay_Mode Mode; - static const Mode POINTS = GLOverlay_Mode_POINTS; - static const Mode LINES = GLOverlay_Mode_LINES; - static const Mode LINE_STRIP = GLOverlay_Mode_LINE_STRIP; - static const Mode LINE_LOOP = GLOverlay_Mode_LINE_LOOP; - static const Mode TRIANGLES = GLOverlay_Mode_TRIANGLES; - static const Mode TRIANGLE_STRIP = GLOverlay_Mode_TRIANGLE_STRIP; - static const Mode TRIANGLE_FAN = GLOverlay_Mode_TRIANGLE_FAN; - static const Mode QUADS = GLOverlay_Mode_QUADS; - static const Mode QUAD_STRIP = GLOverlay_Mode_QUAD_STRIP; - static const Mode POLYGON = GLOverlay_Mode_POLYGON; - static const Mode SOLID_CIRCLE = GLOverlay_Mode_SOLID_CIRCLE; - static const Mode WIRE_CIRCLE = GLOverlay_Mode_WIRE_CIRCLE; - static const Mode SOLID_CUBE = GLOverlay_Mode_SOLID_CUBE; - static const Mode WIRE_CUBE = GLOverlay_Mode_WIRE_CUBE; - static inline bool Mode_IsValid(int value) { - return GLOverlay_Mode_IsValid(value); - } - static const Mode Mode_MIN = - GLOverlay_Mode_Mode_MIN; - static const Mode Mode_MAX = - GLOverlay_Mode_Mode_MAX; - static const int Mode_ARRAYSIZE = - GLOverlay_Mode_Mode_ARRAYSIZE; - static inline const ::google::protobuf::EnumDescriptor* - Mode_descriptor() { - return GLOverlay_Mode_descriptor(); - } - static inline const ::std::string& Mode_Name(Mode value) { - return GLOverlay_Mode_Name(value); - } - static inline bool Mode_Parse(const ::std::string& name, - Mode* value) { - return GLOverlay_Mode_Parse(name, value); - } - - typedef GLOverlay_Identifier Identifier; - static const Identifier END = GLOverlay_Identifier_END; - static const Identifier VERTEX2F = GLOverlay_Identifier_VERTEX2F; - static const Identifier VERTEX3F = GLOverlay_Identifier_VERTEX3F; - static const Identifier ROTATEF = GLOverlay_Identifier_ROTATEF; - static const Identifier TRANSLATEF = GLOverlay_Identifier_TRANSLATEF; - static const Identifier SCALEF = GLOverlay_Identifier_SCALEF; - static const Identifier PUSH_MATRIX = GLOverlay_Identifier_PUSH_MATRIX; - static const Identifier POP_MATRIX = GLOverlay_Identifier_POP_MATRIX; - static const Identifier COLOR3F = GLOverlay_Identifier_COLOR3F; - static const Identifier COLOR4F = GLOverlay_Identifier_COLOR4F; - static const Identifier POINTSIZE = GLOverlay_Identifier_POINTSIZE; - static const Identifier LINEWIDTH = GLOverlay_Identifier_LINEWIDTH; - static inline bool Identifier_IsValid(int value) { - return GLOverlay_Identifier_IsValid(value); - } - static const Identifier Identifier_MIN = - GLOverlay_Identifier_Identifier_MIN; - static const Identifier Identifier_MAX = - GLOverlay_Identifier_Identifier_MAX; - static const int Identifier_ARRAYSIZE = - GLOverlay_Identifier_Identifier_ARRAYSIZE; - static inline const ::google::protobuf::EnumDescriptor* - Identifier_descriptor() { - return GLOverlay_Identifier_descriptor(); - } - static inline const ::std::string& Identifier_Name(Identifier value) { - return GLOverlay_Identifier_Name(value); - } - static inline bool Identifier_Parse(const ::std::string& name, - Identifier* value) { - return GLOverlay_Identifier_Parse(name, value); - } - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // optional string name = 2; - inline bool has_name() const; - inline void clear_name(); - static const int kNameFieldNumber = 2; - inline const ::std::string& name() const; - inline void set_name(const ::std::string& value); - inline void set_name(const char* value); - inline void set_name(const char* value, size_t size); - inline ::std::string* mutable_name(); - inline ::std::string* release_name(); - - // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; - inline bool has_coordinateframetype() const; - inline void clear_coordinateframetype(); - static const int kCoordinateFrameTypeFieldNumber = 3; - inline ::px::GLOverlay_CoordinateFrameType coordinateframetype() const; - inline void set_coordinateframetype(::px::GLOverlay_CoordinateFrameType value); - - // optional double origin_x = 4; - inline bool has_origin_x() const; - inline void clear_origin_x(); - static const int kOriginXFieldNumber = 4; - inline double origin_x() const; - inline void set_origin_x(double value); - - // optional double origin_y = 5; - inline bool has_origin_y() const; - inline void clear_origin_y(); - static const int kOriginYFieldNumber = 5; - inline double origin_y() const; - inline void set_origin_y(double value); - - // optional double origin_z = 6; - inline bool has_origin_z() const; - inline void clear_origin_z(); - static const int kOriginZFieldNumber = 6; - inline double origin_z() const; - inline void set_origin_z(double value); - - // optional bytes data = 7; - inline bool has_data() const; - inline void clear_data(); - static const int kDataFieldNumber = 7; - inline const ::std::string& data() const; - inline void set_data(const ::std::string& value); - inline void set_data(const char* value); - inline void set_data(const void* value, size_t size); - inline ::std::string* mutable_data(); - inline ::std::string* release_data(); - - // @@protoc_insertion_point(class_scope:px.GLOverlay) - private: - inline void set_has_header(); - inline void clear_has_header(); - inline void set_has_name(); - inline void clear_has_name(); - inline void set_has_coordinateframetype(); - inline void clear_has_coordinateframetype(); - inline void set_has_origin_x(); - inline void clear_has_origin_x(); - inline void set_has_origin_y(); - inline void clear_has_origin_y(); - inline void set_has_origin_z(); - inline void clear_has_origin_z(); - inline void set_has_data(); - inline void clear_has_data(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::std::string* name_; - double origin_x_; - double origin_y_; - double origin_z_; - ::std::string* data_; - int coordinateframetype_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(7 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static GLOverlay* default_instance_; -}; -// ------------------------------------------------------------------- - -class Obstacle : public ::google::protobuf::Message { - public: - Obstacle(); - virtual ~Obstacle(); - - Obstacle(const Obstacle& from); - - inline Obstacle& operator=(const Obstacle& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const Obstacle& default_instance(); - - void Swap(Obstacle* other); - - // implements Message ---------------------------------------------- - - Obstacle* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const Obstacle& from); - void MergeFrom(const Obstacle& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // optional float x = 1; - inline bool has_x() const; - inline void clear_x(); - static const int kXFieldNumber = 1; - inline float x() const; - inline void set_x(float value); - - // optional float y = 2; - inline bool has_y() const; - inline void clear_y(); - static const int kYFieldNumber = 2; - inline float y() const; - inline void set_y(float value); - - // optional float z = 3; - inline bool has_z() const; - inline void clear_z(); - static const int kZFieldNumber = 3; - inline float z() const; - inline void set_z(float value); - - // optional float length = 4; - inline bool has_length() const; - inline void clear_length(); - static const int kLengthFieldNumber = 4; - inline float length() const; - inline void set_length(float value); - - // optional float width = 5; - inline bool has_width() const; - inline void clear_width(); - static const int kWidthFieldNumber = 5; - inline float width() const; - inline void set_width(float value); - - // optional float height = 6; - inline bool has_height() const; - inline void clear_height(); - static const int kHeightFieldNumber = 6; - inline float height() const; - inline void set_height(float value); - - // @@protoc_insertion_point(class_scope:px.Obstacle) - private: - inline void set_has_x(); - inline void clear_has_x(); - inline void set_has_y(); - inline void clear_has_y(); - inline void set_has_z(); - inline void clear_has_z(); - inline void set_has_length(); - inline void clear_has_length(); - inline void set_has_width(); - inline void clear_has_width(); - inline void set_has_height(); - inline void clear_has_height(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - float x_; - float y_; - float z_; - float length_; - float width_; - float height_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(6 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static Obstacle* default_instance_; -}; -// ------------------------------------------------------------------- - -class ObstacleList : public ::google::protobuf::Message { - public: - ObstacleList(); - virtual ~ObstacleList(); - - ObstacleList(const ObstacleList& from); - - inline ObstacleList& operator=(const ObstacleList& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const ObstacleList& default_instance(); - - void Swap(ObstacleList* other); - - // implements Message ---------------------------------------------- - - ObstacleList* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const ObstacleList& from); - void MergeFrom(const ObstacleList& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // repeated .px.Obstacle obstacles = 2; - inline int obstacles_size() const; - inline void clear_obstacles(); - static const int kObstaclesFieldNumber = 2; - inline const ::px::Obstacle& obstacles(int index) const; - inline ::px::Obstacle* mutable_obstacles(int index); - inline ::px::Obstacle* add_obstacles(); - inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >& - obstacles() const; - inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >* - mutable_obstacles(); - - // @@protoc_insertion_point(class_scope:px.ObstacleList) - private: - inline void set_has_header(); - inline void clear_has_header(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::RepeatedPtrField< ::px::Obstacle > obstacles_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static ObstacleList* default_instance_; -}; -// ------------------------------------------------------------------- - -class ObstacleMap : public ::google::protobuf::Message { - public: - ObstacleMap(); - virtual ~ObstacleMap(); - - ObstacleMap(const ObstacleMap& from); - - inline ObstacleMap& operator=(const ObstacleMap& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const ObstacleMap& default_instance(); - - void Swap(ObstacleMap* other); - - // implements Message ---------------------------------------------- - - ObstacleMap* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const ObstacleMap& from); - void MergeFrom(const ObstacleMap& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // required int32 type = 2; - inline bool has_type() const; - inline void clear_type(); - static const int kTypeFieldNumber = 2; - inline ::google::protobuf::int32 type() const; - inline void set_type(::google::protobuf::int32 value); - - // optional float resolution = 3; - inline bool has_resolution() const; - inline void clear_resolution(); - static const int kResolutionFieldNumber = 3; - inline float resolution() const; - inline void set_resolution(float value); - - // optional int32 rows = 4; - inline bool has_rows() const; - inline void clear_rows(); - static const int kRowsFieldNumber = 4; - inline ::google::protobuf::int32 rows() const; - inline void set_rows(::google::protobuf::int32 value); - - // optional int32 cols = 5; - inline bool has_cols() const; - inline void clear_cols(); - static const int kColsFieldNumber = 5; - inline ::google::protobuf::int32 cols() const; - inline void set_cols(::google::protobuf::int32 value); - - // optional int32 mapR0 = 6; - inline bool has_mapr0() const; - inline void clear_mapr0(); - static const int kMapR0FieldNumber = 6; - inline ::google::protobuf::int32 mapr0() const; - inline void set_mapr0(::google::protobuf::int32 value); - - // optional int32 mapC0 = 7; - inline bool has_mapc0() const; - inline void clear_mapc0(); - static const int kMapC0FieldNumber = 7; - inline ::google::protobuf::int32 mapc0() const; - inline void set_mapc0(::google::protobuf::int32 value); - - // optional int32 arrayR0 = 8; - inline bool has_arrayr0() const; - inline void clear_arrayr0(); - static const int kArrayR0FieldNumber = 8; - inline ::google::protobuf::int32 arrayr0() const; - inline void set_arrayr0(::google::protobuf::int32 value); - - // optional int32 arrayC0 = 9; - inline bool has_arrayc0() const; - inline void clear_arrayc0(); - static const int kArrayC0FieldNumber = 9; - inline ::google::protobuf::int32 arrayc0() const; - inline void set_arrayc0(::google::protobuf::int32 value); - - // optional bytes data = 10; - inline bool has_data() const; - inline void clear_data(); - static const int kDataFieldNumber = 10; - inline const ::std::string& data() const; - inline void set_data(const ::std::string& value); - inline void set_data(const char* value); - inline void set_data(const void* value, size_t size); - inline ::std::string* mutable_data(); - inline ::std::string* release_data(); - - // @@protoc_insertion_point(class_scope:px.ObstacleMap) - private: - inline void set_has_header(); - inline void clear_has_header(); - inline void set_has_type(); - inline void clear_has_type(); - inline void set_has_resolution(); - inline void clear_has_resolution(); - inline void set_has_rows(); - inline void clear_has_rows(); - inline void set_has_cols(); - inline void clear_has_cols(); - inline void set_has_mapr0(); - inline void clear_has_mapr0(); - inline void set_has_mapc0(); - inline void clear_has_mapc0(); - inline void set_has_arrayr0(); - inline void clear_has_arrayr0(); - inline void set_has_arrayc0(); - inline void clear_has_arrayc0(); - inline void set_has_data(); - inline void clear_has_data(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::int32 type_; - float resolution_; - ::google::protobuf::int32 rows_; - ::google::protobuf::int32 cols_; - ::google::protobuf::int32 mapr0_; - ::google::protobuf::int32 mapc0_; - ::google::protobuf::int32 arrayr0_; - ::google::protobuf::int32 arrayc0_; - ::std::string* data_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(10 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static ObstacleMap* default_instance_; -}; -// ------------------------------------------------------------------- - -class Path : public ::google::protobuf::Message { - public: - Path(); - virtual ~Path(); - - Path(const Path& from); - - inline Path& operator=(const Path& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const Path& default_instance(); - - void Swap(Path* other); - - // implements Message ---------------------------------------------- - - Path* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const Path& from); - void MergeFrom(const Path& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // repeated .px.Waypoint waypoints = 2; - inline int waypoints_size() const; - inline void clear_waypoints(); - static const int kWaypointsFieldNumber = 2; - inline const ::px::Waypoint& waypoints(int index) const; - inline ::px::Waypoint* mutable_waypoints(int index); - inline ::px::Waypoint* add_waypoints(); - inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >& - waypoints() const; - inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >* - mutable_waypoints(); - - // @@protoc_insertion_point(class_scope:px.Path) - private: - inline void set_has_header(); - inline void clear_has_header(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::RepeatedPtrField< ::px::Waypoint > waypoints_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static Path* default_instance_; -}; -// ------------------------------------------------------------------- - -class PointCloudXYZI_PointXYZI : public ::google::protobuf::Message { - public: - PointCloudXYZI_PointXYZI(); - virtual ~PointCloudXYZI_PointXYZI(); - - PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from); - - inline PointCloudXYZI_PointXYZI& operator=(const PointCloudXYZI_PointXYZI& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const PointCloudXYZI_PointXYZI& default_instance(); - - void Swap(PointCloudXYZI_PointXYZI* other); - - // implements Message ---------------------------------------------- - - PointCloudXYZI_PointXYZI* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const PointCloudXYZI_PointXYZI& from); - void MergeFrom(const PointCloudXYZI_PointXYZI& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required float x = 1; - inline bool has_x() const; - inline void clear_x(); - static const int kXFieldNumber = 1; - inline float x() const; - inline void set_x(float value); - - // required float y = 2; - inline bool has_y() const; - inline void clear_y(); - static const int kYFieldNumber = 2; - inline float y() const; - inline void set_y(float value); - - // required float z = 3; - inline bool has_z() const; - inline void clear_z(); - static const int kZFieldNumber = 3; - inline float z() const; - inline void set_z(float value); - - // required float intensity = 4; - inline bool has_intensity() const; - inline void clear_intensity(); - static const int kIntensityFieldNumber = 4; - inline float intensity() const; - inline void set_intensity(float value); - - // @@protoc_insertion_point(class_scope:px.PointCloudXYZI.PointXYZI) - private: - inline void set_has_x(); - inline void clear_has_x(); - inline void set_has_y(); - inline void clear_has_y(); - inline void set_has_z(); - inline void clear_has_z(); - inline void set_has_intensity(); - inline void clear_has_intensity(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - float x_; - float y_; - float z_; - float intensity_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(4 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static PointCloudXYZI_PointXYZI* default_instance_; -}; -// ------------------------------------------------------------------- - -class PointCloudXYZI : public ::google::protobuf::Message { - public: - PointCloudXYZI(); - virtual ~PointCloudXYZI(); - - PointCloudXYZI(const PointCloudXYZI& from); - - inline PointCloudXYZI& operator=(const PointCloudXYZI& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const PointCloudXYZI& default_instance(); - - void Swap(PointCloudXYZI* other); - - // implements Message ---------------------------------------------- - - PointCloudXYZI* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const PointCloudXYZI& from); - void MergeFrom(const PointCloudXYZI& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - typedef PointCloudXYZI_PointXYZI PointXYZI; - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // repeated .px.PointCloudXYZI.PointXYZI points = 2; - inline int points_size() const; - inline void clear_points(); - static const int kPointsFieldNumber = 2; - inline const ::px::PointCloudXYZI_PointXYZI& points(int index) const; - inline ::px::PointCloudXYZI_PointXYZI* mutable_points(int index); - inline ::px::PointCloudXYZI_PointXYZI* add_points(); - inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >& - points() const; - inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >* - mutable_points(); - - // @@protoc_insertion_point(class_scope:px.PointCloudXYZI) - private: - inline void set_has_header(); - inline void clear_has_header(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI > points_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static PointCloudXYZI* default_instance_; -}; -// ------------------------------------------------------------------- - -class PointCloudXYZRGB_PointXYZRGB : public ::google::protobuf::Message { - public: - PointCloudXYZRGB_PointXYZRGB(); - virtual ~PointCloudXYZRGB_PointXYZRGB(); - - PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from); - - inline PointCloudXYZRGB_PointXYZRGB& operator=(const PointCloudXYZRGB_PointXYZRGB& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const PointCloudXYZRGB_PointXYZRGB& default_instance(); - - void Swap(PointCloudXYZRGB_PointXYZRGB* other); - - // implements Message ---------------------------------------------- - - PointCloudXYZRGB_PointXYZRGB* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from); - void MergeFrom(const PointCloudXYZRGB_PointXYZRGB& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required float x = 1; - inline bool has_x() const; - inline void clear_x(); - static const int kXFieldNumber = 1; - inline float x() const; - inline void set_x(float value); - - // required float y = 2; - inline bool has_y() const; - inline void clear_y(); - static const int kYFieldNumber = 2; - inline float y() const; - inline void set_y(float value); - - // required float z = 3; - inline bool has_z() const; - inline void clear_z(); - static const int kZFieldNumber = 3; - inline float z() const; - inline void set_z(float value); - - // required float rgb = 4; - inline bool has_rgb() const; - inline void clear_rgb(); - static const int kRgbFieldNumber = 4; - inline float rgb() const; - inline void set_rgb(float value); - - // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB.PointXYZRGB) - private: - inline void set_has_x(); - inline void clear_has_x(); - inline void set_has_y(); - inline void clear_has_y(); - inline void set_has_z(); - inline void clear_has_z(); - inline void set_has_rgb(); - inline void clear_has_rgb(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - float x_; - float y_; - float z_; - float rgb_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(4 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static PointCloudXYZRGB_PointXYZRGB* default_instance_; -}; -// ------------------------------------------------------------------- - -class PointCloudXYZRGB : public ::google::protobuf::Message { - public: - PointCloudXYZRGB(); - virtual ~PointCloudXYZRGB(); - - PointCloudXYZRGB(const PointCloudXYZRGB& from); - - inline PointCloudXYZRGB& operator=(const PointCloudXYZRGB& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const PointCloudXYZRGB& default_instance(); - - void Swap(PointCloudXYZRGB* other); - - // implements Message ---------------------------------------------- - - PointCloudXYZRGB* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const PointCloudXYZRGB& from); - void MergeFrom(const PointCloudXYZRGB& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - typedef PointCloudXYZRGB_PointXYZRGB PointXYZRGB; - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; - inline int points_size() const; - inline void clear_points(); - static const int kPointsFieldNumber = 2; - inline const ::px::PointCloudXYZRGB_PointXYZRGB& points(int index) const; - inline ::px::PointCloudXYZRGB_PointXYZRGB* mutable_points(int index); - inline ::px::PointCloudXYZRGB_PointXYZRGB* add_points(); - inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >& - points() const; - inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >* - mutable_points(); - - // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB) - private: - inline void set_has_header(); - inline void clear_has_header(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB > points_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static PointCloudXYZRGB* default_instance_; -}; -// ------------------------------------------------------------------- - -class RGBDImage : public ::google::protobuf::Message { - public: - RGBDImage(); - virtual ~RGBDImage(); - - RGBDImage(const RGBDImage& from); - - inline RGBDImage& operator=(const RGBDImage& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const RGBDImage& default_instance(); - - void Swap(RGBDImage* other); - - // implements Message ---------------------------------------------- - - RGBDImage* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const RGBDImage& from); - void MergeFrom(const RGBDImage& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // required uint32 cols = 2; - inline bool has_cols() const; - inline void clear_cols(); - static const int kColsFieldNumber = 2; - inline ::google::protobuf::uint32 cols() const; - inline void set_cols(::google::protobuf::uint32 value); - - // required uint32 rows = 3; - inline bool has_rows() const; - inline void clear_rows(); - static const int kRowsFieldNumber = 3; - inline ::google::protobuf::uint32 rows() const; - inline void set_rows(::google::protobuf::uint32 value); - - // required uint32 step1 = 4; - inline bool has_step1() const; - inline void clear_step1(); - static const int kStep1FieldNumber = 4; - inline ::google::protobuf::uint32 step1() const; - inline void set_step1(::google::protobuf::uint32 value); - - // required uint32 type1 = 5; - inline bool has_type1() const; - inline void clear_type1(); - static const int kType1FieldNumber = 5; - inline ::google::protobuf::uint32 type1() const; - inline void set_type1(::google::protobuf::uint32 value); - - // required bytes imageData1 = 6; - inline bool has_imagedata1() const; - inline void clear_imagedata1(); - static const int kImageData1FieldNumber = 6; - inline const ::std::string& imagedata1() const; - inline void set_imagedata1(const ::std::string& value); - inline void set_imagedata1(const char* value); - inline void set_imagedata1(const void* value, size_t size); - inline ::std::string* mutable_imagedata1(); - inline ::std::string* release_imagedata1(); - - // required uint32 step2 = 7; - inline bool has_step2() const; - inline void clear_step2(); - static const int kStep2FieldNumber = 7; - inline ::google::protobuf::uint32 step2() const; - inline void set_step2(::google::protobuf::uint32 value); - - // required uint32 type2 = 8; - inline bool has_type2() const; - inline void clear_type2(); - static const int kType2FieldNumber = 8; - inline ::google::protobuf::uint32 type2() const; - inline void set_type2(::google::protobuf::uint32 value); - - // required bytes imageData2 = 9; - inline bool has_imagedata2() const; - inline void clear_imagedata2(); - static const int kImageData2FieldNumber = 9; - inline const ::std::string& imagedata2() const; - inline void set_imagedata2(const ::std::string& value); - inline void set_imagedata2(const char* value); - inline void set_imagedata2(const void* value, size_t size); - inline ::std::string* mutable_imagedata2(); - inline ::std::string* release_imagedata2(); - - // optional uint32 camera_config = 10; - inline bool has_camera_config() const; - inline void clear_camera_config(); - static const int kCameraConfigFieldNumber = 10; - inline ::google::protobuf::uint32 camera_config() const; - inline void set_camera_config(::google::protobuf::uint32 value); - - // optional uint32 camera_type = 11; - inline bool has_camera_type() const; - inline void clear_camera_type(); - static const int kCameraTypeFieldNumber = 11; - inline ::google::protobuf::uint32 camera_type() const; - inline void set_camera_type(::google::protobuf::uint32 value); - - // optional float roll = 12; - inline bool has_roll() const; - inline void clear_roll(); - static const int kRollFieldNumber = 12; - inline float roll() const; - inline void set_roll(float value); - - // optional float pitch = 13; - inline bool has_pitch() const; - inline void clear_pitch(); - static const int kPitchFieldNumber = 13; - inline float pitch() const; - inline void set_pitch(float value); - - // optional float yaw = 14; - inline bool has_yaw() const; - inline void clear_yaw(); - static const int kYawFieldNumber = 14; - inline float yaw() const; - inline void set_yaw(float value); - - // optional float lon = 15; - inline bool has_lon() const; - inline void clear_lon(); - static const int kLonFieldNumber = 15; - inline float lon() const; - inline void set_lon(float value); - - // optional float lat = 16; - inline bool has_lat() const; - inline void clear_lat(); - static const int kLatFieldNumber = 16; - inline float lat() const; - inline void set_lat(float value); - - // optional float alt = 17; - inline bool has_alt() const; - inline void clear_alt(); - static const int kAltFieldNumber = 17; - inline float alt() const; - inline void set_alt(float value); - - // optional float ground_x = 18; - inline bool has_ground_x() const; - inline void clear_ground_x(); - static const int kGroundXFieldNumber = 18; - inline float ground_x() const; - inline void set_ground_x(float value); - - // optional float ground_y = 19; - inline bool has_ground_y() const; - inline void clear_ground_y(); - static const int kGroundYFieldNumber = 19; - inline float ground_y() const; - inline void set_ground_y(float value); - - // optional float ground_z = 20; - inline bool has_ground_z() const; - inline void clear_ground_z(); - static const int kGroundZFieldNumber = 20; - inline float ground_z() const; - inline void set_ground_z(float value); - - // repeated float camera_matrix = 21; - inline int camera_matrix_size() const; - inline void clear_camera_matrix(); - static const int kCameraMatrixFieldNumber = 21; - inline float camera_matrix(int index) const; - inline void set_camera_matrix(int index, float value); - inline void add_camera_matrix(float value); - inline const ::google::protobuf::RepeatedField< float >& - camera_matrix() const; - inline ::google::protobuf::RepeatedField< float >* - mutable_camera_matrix(); - - // @@protoc_insertion_point(class_scope:px.RGBDImage) - private: - inline void set_has_header(); - inline void clear_has_header(); - inline void set_has_cols(); - inline void clear_has_cols(); - inline void set_has_rows(); - inline void clear_has_rows(); - inline void set_has_step1(); - inline void clear_has_step1(); - inline void set_has_type1(); - inline void clear_has_type1(); - inline void set_has_imagedata1(); - inline void clear_has_imagedata1(); - inline void set_has_step2(); - inline void clear_has_step2(); - inline void set_has_type2(); - inline void clear_has_type2(); - inline void set_has_imagedata2(); - inline void clear_has_imagedata2(); - inline void set_has_camera_config(); - inline void clear_has_camera_config(); - inline void set_has_camera_type(); - inline void clear_has_camera_type(); - inline void set_has_roll(); - inline void clear_has_roll(); - inline void set_has_pitch(); - inline void clear_has_pitch(); - inline void set_has_yaw(); - inline void clear_has_yaw(); - inline void set_has_lon(); - inline void clear_has_lon(); - inline void set_has_lat(); - inline void clear_has_lat(); - inline void set_has_alt(); - inline void clear_has_alt(); - inline void set_has_ground_x(); - inline void clear_has_ground_x(); - inline void set_has_ground_y(); - inline void clear_has_ground_y(); - inline void set_has_ground_z(); - inline void clear_has_ground_z(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::uint32 cols_; - ::google::protobuf::uint32 rows_; - ::google::protobuf::uint32 step1_; - ::google::protobuf::uint32 type1_; - ::std::string* imagedata1_; - ::google::protobuf::uint32 step2_; - ::google::protobuf::uint32 type2_; - ::std::string* imagedata2_; - ::google::protobuf::uint32 camera_config_; - ::google::protobuf::uint32 camera_type_; - float roll_; - float pitch_; - float yaw_; - float lon_; - float lat_; - float alt_; - float ground_x_; - float ground_y_; - ::google::protobuf::RepeatedField< float > camera_matrix_; - float ground_z_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(21 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static RGBDImage* default_instance_; -}; -// ------------------------------------------------------------------- - -class Waypoint : public ::google::protobuf::Message { - public: - Waypoint(); - virtual ~Waypoint(); - - Waypoint(const Waypoint& from); - - inline Waypoint& operator=(const Waypoint& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const Waypoint& default_instance(); - - void Swap(Waypoint* other); - - // implements Message ---------------------------------------------- - - Waypoint* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const Waypoint& from); - void MergeFrom(const Waypoint& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required double x = 1; - inline bool has_x() const; - inline void clear_x(); - static const int kXFieldNumber = 1; - inline double x() const; - inline void set_x(double value); - - // required double y = 2; - inline bool has_y() const; - inline void clear_y(); - static const int kYFieldNumber = 2; - inline double y() const; - inline void set_y(double value); - - // optional double z = 3; - inline bool has_z() const; - inline void clear_z(); - static const int kZFieldNumber = 3; - inline double z() const; - inline void set_z(double value); - - // optional double roll = 4; - inline bool has_roll() const; - inline void clear_roll(); - static const int kRollFieldNumber = 4; - inline double roll() const; - inline void set_roll(double value); - - // optional double pitch = 5; - inline bool has_pitch() const; - inline void clear_pitch(); - static const int kPitchFieldNumber = 5; - inline double pitch() const; - inline void set_pitch(double value); - - // optional double yaw = 6; - inline bool has_yaw() const; - inline void clear_yaw(); - static const int kYawFieldNumber = 6; - inline double yaw() const; - inline void set_yaw(double value); - - // @@protoc_insertion_point(class_scope:px.Waypoint) - private: - inline void set_has_x(); - inline void clear_has_x(); - inline void set_has_y(); - inline void clear_has_y(); - inline void set_has_z(); - inline void clear_has_z(); - inline void set_has_roll(); - inline void clear_has_roll(); - inline void set_has_pitch(); - inline void clear_has_pitch(); - inline void set_has_yaw(); - inline void clear_has_yaw(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - double x_; - double y_; - double z_; - double roll_; - double pitch_; - double yaw_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(6 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static Waypoint* default_instance_; -}; -// =================================================================== - - -// =================================================================== - -// HeaderInfo - -// required int32 source_sysid = 1; -inline bool HeaderInfo::has_source_sysid() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void HeaderInfo::set_has_source_sysid() { - _has_bits_[0] |= 0x00000001u; -} -inline void HeaderInfo::clear_has_source_sysid() { - _has_bits_[0] &= ~0x00000001u; -} -inline void HeaderInfo::clear_source_sysid() { - source_sysid_ = 0; - clear_has_source_sysid(); -} -inline ::google::protobuf::int32 HeaderInfo::source_sysid() const { - return source_sysid_; -} -inline void HeaderInfo::set_source_sysid(::google::protobuf::int32 value) { - set_has_source_sysid(); - source_sysid_ = value; -} - -// required int32 source_compid = 2; -inline bool HeaderInfo::has_source_compid() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void HeaderInfo::set_has_source_compid() { - _has_bits_[0] |= 0x00000002u; -} -inline void HeaderInfo::clear_has_source_compid() { - _has_bits_[0] &= ~0x00000002u; -} -inline void HeaderInfo::clear_source_compid() { - source_compid_ = 0; - clear_has_source_compid(); -} -inline ::google::protobuf::int32 HeaderInfo::source_compid() const { - return source_compid_; -} -inline void HeaderInfo::set_source_compid(::google::protobuf::int32 value) { - set_has_source_compid(); - source_compid_ = value; -} - -// required double timestamp = 3; -inline bool HeaderInfo::has_timestamp() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void HeaderInfo::set_has_timestamp() { - _has_bits_[0] |= 0x00000004u; -} -inline void HeaderInfo::clear_has_timestamp() { - _has_bits_[0] &= ~0x00000004u; -} -inline void HeaderInfo::clear_timestamp() { - timestamp_ = 0; - clear_has_timestamp(); -} -inline double HeaderInfo::timestamp() const { - return timestamp_; -} -inline void HeaderInfo::set_timestamp(double value) { - set_has_timestamp(); - timestamp_ = value; -} - -// ------------------------------------------------------------------- - -// GLOverlay - -// required .px.HeaderInfo header = 1; -inline bool GLOverlay::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void GLOverlay::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void GLOverlay::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void GLOverlay::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& GLOverlay::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* GLOverlay::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* GLOverlay::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// optional string name = 2; -inline bool GLOverlay::has_name() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void GLOverlay::set_has_name() { - _has_bits_[0] |= 0x00000002u; -} -inline void GLOverlay::clear_has_name() { - _has_bits_[0] &= ~0x00000002u; -} -inline void GLOverlay::clear_name() { - if (name_ != &::google::protobuf::internal::kEmptyString) { - name_->clear(); - } - clear_has_name(); -} -inline const ::std::string& GLOverlay::name() const { - return *name_; -} -inline void GLOverlay::set_name(const ::std::string& value) { - set_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - name_ = new ::std::string; - } - name_->assign(value); -} -inline void GLOverlay::set_name(const char* value) { - set_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - name_ = new ::std::string; - } - name_->assign(value); -} -inline void GLOverlay::set_name(const char* value, size_t size) { - set_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - name_ = new ::std::string; - } - name_->assign(reinterpret_cast(value), size); -} -inline ::std::string* GLOverlay::mutable_name() { - set_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - name_ = new ::std::string; - } - return name_; -} -inline ::std::string* GLOverlay::release_name() { - clear_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = name_; - name_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; -inline bool GLOverlay::has_coordinateframetype() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void GLOverlay::set_has_coordinateframetype() { - _has_bits_[0] |= 0x00000004u; -} -inline void GLOverlay::clear_has_coordinateframetype() { - _has_bits_[0] &= ~0x00000004u; -} -inline void GLOverlay::clear_coordinateframetype() { - coordinateframetype_ = 0; - clear_has_coordinateframetype(); -} -inline ::px::GLOverlay_CoordinateFrameType GLOverlay::coordinateframetype() const { - return static_cast< ::px::GLOverlay_CoordinateFrameType >(coordinateframetype_); -} -inline void GLOverlay::set_coordinateframetype(::px::GLOverlay_CoordinateFrameType value) { - GOOGLE_DCHECK(::px::GLOverlay_CoordinateFrameType_IsValid(value)); - set_has_coordinateframetype(); - coordinateframetype_ = value; -} - -// optional double origin_x = 4; -inline bool GLOverlay::has_origin_x() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void GLOverlay::set_has_origin_x() { - _has_bits_[0] |= 0x00000008u; -} -inline void GLOverlay::clear_has_origin_x() { - _has_bits_[0] &= ~0x00000008u; -} -inline void GLOverlay::clear_origin_x() { - origin_x_ = 0; - clear_has_origin_x(); -} -inline double GLOverlay::origin_x() const { - return origin_x_; -} -inline void GLOverlay::set_origin_x(double value) { - set_has_origin_x(); - origin_x_ = value; -} - -// optional double origin_y = 5; -inline bool GLOverlay::has_origin_y() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void GLOverlay::set_has_origin_y() { - _has_bits_[0] |= 0x00000010u; -} -inline void GLOverlay::clear_has_origin_y() { - _has_bits_[0] &= ~0x00000010u; -} -inline void GLOverlay::clear_origin_y() { - origin_y_ = 0; - clear_has_origin_y(); -} -inline double GLOverlay::origin_y() const { - return origin_y_; -} -inline void GLOverlay::set_origin_y(double value) { - set_has_origin_y(); - origin_y_ = value; -} - -// optional double origin_z = 6; -inline bool GLOverlay::has_origin_z() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void GLOverlay::set_has_origin_z() { - _has_bits_[0] |= 0x00000020u; -} -inline void GLOverlay::clear_has_origin_z() { - _has_bits_[0] &= ~0x00000020u; -} -inline void GLOverlay::clear_origin_z() { - origin_z_ = 0; - clear_has_origin_z(); -} -inline double GLOverlay::origin_z() const { - return origin_z_; -} -inline void GLOverlay::set_origin_z(double value) { - set_has_origin_z(); - origin_z_ = value; -} - -// optional bytes data = 7; -inline bool GLOverlay::has_data() const { - return (_has_bits_[0] & 0x00000040u) != 0; -} -inline void GLOverlay::set_has_data() { - _has_bits_[0] |= 0x00000040u; -} -inline void GLOverlay::clear_has_data() { - _has_bits_[0] &= ~0x00000040u; -} -inline void GLOverlay::clear_data() { - if (data_ != &::google::protobuf::internal::kEmptyString) { - data_->clear(); - } - clear_has_data(); -} -inline const ::std::string& GLOverlay::data() const { - return *data_; -} -inline void GLOverlay::set_data(const ::std::string& value) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(value); -} -inline void GLOverlay::set_data(const char* value) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(value); -} -inline void GLOverlay::set_data(const void* value, size_t size) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(reinterpret_cast(value), size); -} -inline ::std::string* GLOverlay::mutable_data() { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - return data_; -} -inline ::std::string* GLOverlay::release_data() { - clear_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = data_; - data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// ------------------------------------------------------------------- - -// Obstacle - -// optional float x = 1; -inline bool Obstacle::has_x() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void Obstacle::set_has_x() { - _has_bits_[0] |= 0x00000001u; -} -inline void Obstacle::clear_has_x() { - _has_bits_[0] &= ~0x00000001u; -} -inline void Obstacle::clear_x() { - x_ = 0; - clear_has_x(); -} -inline float Obstacle::x() const { - return x_; -} -inline void Obstacle::set_x(float value) { - set_has_x(); - x_ = value; -} - -// optional float y = 2; -inline bool Obstacle::has_y() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void Obstacle::set_has_y() { - _has_bits_[0] |= 0x00000002u; -} -inline void Obstacle::clear_has_y() { - _has_bits_[0] &= ~0x00000002u; -} -inline void Obstacle::clear_y() { - y_ = 0; - clear_has_y(); -} -inline float Obstacle::y() const { - return y_; -} -inline void Obstacle::set_y(float value) { - set_has_y(); - y_ = value; -} - -// optional float z = 3; -inline bool Obstacle::has_z() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void Obstacle::set_has_z() { - _has_bits_[0] |= 0x00000004u; -} -inline void Obstacle::clear_has_z() { - _has_bits_[0] &= ~0x00000004u; -} -inline void Obstacle::clear_z() { - z_ = 0; - clear_has_z(); -} -inline float Obstacle::z() const { - return z_; -} -inline void Obstacle::set_z(float value) { - set_has_z(); - z_ = value; -} - -// optional float length = 4; -inline bool Obstacle::has_length() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void Obstacle::set_has_length() { - _has_bits_[0] |= 0x00000008u; -} -inline void Obstacle::clear_has_length() { - _has_bits_[0] &= ~0x00000008u; -} -inline void Obstacle::clear_length() { - length_ = 0; - clear_has_length(); -} -inline float Obstacle::length() const { - return length_; -} -inline void Obstacle::set_length(float value) { - set_has_length(); - length_ = value; -} - -// optional float width = 5; -inline bool Obstacle::has_width() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void Obstacle::set_has_width() { - _has_bits_[0] |= 0x00000010u; -} -inline void Obstacle::clear_has_width() { - _has_bits_[0] &= ~0x00000010u; -} -inline void Obstacle::clear_width() { - width_ = 0; - clear_has_width(); -} -inline float Obstacle::width() const { - return width_; -} -inline void Obstacle::set_width(float value) { - set_has_width(); - width_ = value; -} - -// optional float height = 6; -inline bool Obstacle::has_height() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void Obstacle::set_has_height() { - _has_bits_[0] |= 0x00000020u; -} -inline void Obstacle::clear_has_height() { - _has_bits_[0] &= ~0x00000020u; -} -inline void Obstacle::clear_height() { - height_ = 0; - clear_has_height(); -} -inline float Obstacle::height() const { - return height_; -} -inline void Obstacle::set_height(float value) { - set_has_height(); - height_ = value; -} - -// ------------------------------------------------------------------- - -// ObstacleList - -// required .px.HeaderInfo header = 1; -inline bool ObstacleList::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void ObstacleList::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void ObstacleList::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void ObstacleList::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& ObstacleList::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* ObstacleList::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* ObstacleList::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// repeated .px.Obstacle obstacles = 2; -inline int ObstacleList::obstacles_size() const { - return obstacles_.size(); -} -inline void ObstacleList::clear_obstacles() { - obstacles_.Clear(); -} -inline const ::px::Obstacle& ObstacleList::obstacles(int index) const { - return obstacles_.Get(index); -} -inline ::px::Obstacle* ObstacleList::mutable_obstacles(int index) { - return obstacles_.Mutable(index); -} -inline ::px::Obstacle* ObstacleList::add_obstacles() { - return obstacles_.Add(); -} -inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >& -ObstacleList::obstacles() const { - return obstacles_; -} -inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >* -ObstacleList::mutable_obstacles() { - return &obstacles_; -} - -// ------------------------------------------------------------------- - -// ObstacleMap - -// required .px.HeaderInfo header = 1; -inline bool ObstacleMap::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void ObstacleMap::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void ObstacleMap::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void ObstacleMap::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& ObstacleMap::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* ObstacleMap::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* ObstacleMap::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// required int32 type = 2; -inline bool ObstacleMap::has_type() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void ObstacleMap::set_has_type() { - _has_bits_[0] |= 0x00000002u; -} -inline void ObstacleMap::clear_has_type() { - _has_bits_[0] &= ~0x00000002u; -} -inline void ObstacleMap::clear_type() { - type_ = 0; - clear_has_type(); -} -inline ::google::protobuf::int32 ObstacleMap::type() const { - return type_; -} -inline void ObstacleMap::set_type(::google::protobuf::int32 value) { - set_has_type(); - type_ = value; -} - -// optional float resolution = 3; -inline bool ObstacleMap::has_resolution() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void ObstacleMap::set_has_resolution() { - _has_bits_[0] |= 0x00000004u; -} -inline void ObstacleMap::clear_has_resolution() { - _has_bits_[0] &= ~0x00000004u; -} -inline void ObstacleMap::clear_resolution() { - resolution_ = 0; - clear_has_resolution(); -} -inline float ObstacleMap::resolution() const { - return resolution_; -} -inline void ObstacleMap::set_resolution(float value) { - set_has_resolution(); - resolution_ = value; -} - -// optional int32 rows = 4; -inline bool ObstacleMap::has_rows() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void ObstacleMap::set_has_rows() { - _has_bits_[0] |= 0x00000008u; -} -inline void ObstacleMap::clear_has_rows() { - _has_bits_[0] &= ~0x00000008u; -} -inline void ObstacleMap::clear_rows() { - rows_ = 0; - clear_has_rows(); -} -inline ::google::protobuf::int32 ObstacleMap::rows() const { - return rows_; -} -inline void ObstacleMap::set_rows(::google::protobuf::int32 value) { - set_has_rows(); - rows_ = value; -} - -// optional int32 cols = 5; -inline bool ObstacleMap::has_cols() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void ObstacleMap::set_has_cols() { - _has_bits_[0] |= 0x00000010u; -} -inline void ObstacleMap::clear_has_cols() { - _has_bits_[0] &= ~0x00000010u; -} -inline void ObstacleMap::clear_cols() { - cols_ = 0; - clear_has_cols(); -} -inline ::google::protobuf::int32 ObstacleMap::cols() const { - return cols_; -} -inline void ObstacleMap::set_cols(::google::protobuf::int32 value) { - set_has_cols(); - cols_ = value; -} - -// optional int32 mapR0 = 6; -inline bool ObstacleMap::has_mapr0() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void ObstacleMap::set_has_mapr0() { - _has_bits_[0] |= 0x00000020u; -} -inline void ObstacleMap::clear_has_mapr0() { - _has_bits_[0] &= ~0x00000020u; -} -inline void ObstacleMap::clear_mapr0() { - mapr0_ = 0; - clear_has_mapr0(); -} -inline ::google::protobuf::int32 ObstacleMap::mapr0() const { - return mapr0_; -} -inline void ObstacleMap::set_mapr0(::google::protobuf::int32 value) { - set_has_mapr0(); - mapr0_ = value; -} - -// optional int32 mapC0 = 7; -inline bool ObstacleMap::has_mapc0() const { - return (_has_bits_[0] & 0x00000040u) != 0; -} -inline void ObstacleMap::set_has_mapc0() { - _has_bits_[0] |= 0x00000040u; -} -inline void ObstacleMap::clear_has_mapc0() { - _has_bits_[0] &= ~0x00000040u; -} -inline void ObstacleMap::clear_mapc0() { - mapc0_ = 0; - clear_has_mapc0(); -} -inline ::google::protobuf::int32 ObstacleMap::mapc0() const { - return mapc0_; -} -inline void ObstacleMap::set_mapc0(::google::protobuf::int32 value) { - set_has_mapc0(); - mapc0_ = value; -} - -// optional int32 arrayR0 = 8; -inline bool ObstacleMap::has_arrayr0() const { - return (_has_bits_[0] & 0x00000080u) != 0; -} -inline void ObstacleMap::set_has_arrayr0() { - _has_bits_[0] |= 0x00000080u; -} -inline void ObstacleMap::clear_has_arrayr0() { - _has_bits_[0] &= ~0x00000080u; -} -inline void ObstacleMap::clear_arrayr0() { - arrayr0_ = 0; - clear_has_arrayr0(); -} -inline ::google::protobuf::int32 ObstacleMap::arrayr0() const { - return arrayr0_; -} -inline void ObstacleMap::set_arrayr0(::google::protobuf::int32 value) { - set_has_arrayr0(); - arrayr0_ = value; -} - -// optional int32 arrayC0 = 9; -inline bool ObstacleMap::has_arrayc0() const { - return (_has_bits_[0] & 0x00000100u) != 0; -} -inline void ObstacleMap::set_has_arrayc0() { - _has_bits_[0] |= 0x00000100u; -} -inline void ObstacleMap::clear_has_arrayc0() { - _has_bits_[0] &= ~0x00000100u; -} -inline void ObstacleMap::clear_arrayc0() { - arrayc0_ = 0; - clear_has_arrayc0(); -} -inline ::google::protobuf::int32 ObstacleMap::arrayc0() const { - return arrayc0_; -} -inline void ObstacleMap::set_arrayc0(::google::protobuf::int32 value) { - set_has_arrayc0(); - arrayc0_ = value; -} - -// optional bytes data = 10; -inline bool ObstacleMap::has_data() const { - return (_has_bits_[0] & 0x00000200u) != 0; -} -inline void ObstacleMap::set_has_data() { - _has_bits_[0] |= 0x00000200u; -} -inline void ObstacleMap::clear_has_data() { - _has_bits_[0] &= ~0x00000200u; -} -inline void ObstacleMap::clear_data() { - if (data_ != &::google::protobuf::internal::kEmptyString) { - data_->clear(); - } - clear_has_data(); -} -inline const ::std::string& ObstacleMap::data() const { - return *data_; -} -inline void ObstacleMap::set_data(const ::std::string& value) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(value); -} -inline void ObstacleMap::set_data(const char* value) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(value); -} -inline void ObstacleMap::set_data(const void* value, size_t size) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(reinterpret_cast(value), size); -} -inline ::std::string* ObstacleMap::mutable_data() { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - return data_; -} -inline ::std::string* ObstacleMap::release_data() { - clear_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = data_; - data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// ------------------------------------------------------------------- - -// Path - -// required .px.HeaderInfo header = 1; -inline bool Path::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void Path::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void Path::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void Path::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& Path::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* Path::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* Path::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// repeated .px.Waypoint waypoints = 2; -inline int Path::waypoints_size() const { - return waypoints_.size(); -} -inline void Path::clear_waypoints() { - waypoints_.Clear(); -} -inline const ::px::Waypoint& Path::waypoints(int index) const { - return waypoints_.Get(index); -} -inline ::px::Waypoint* Path::mutable_waypoints(int index) { - return waypoints_.Mutable(index); -} -inline ::px::Waypoint* Path::add_waypoints() { - return waypoints_.Add(); -} -inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >& -Path::waypoints() const { - return waypoints_; -} -inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >* -Path::mutable_waypoints() { - return &waypoints_; -} - -// ------------------------------------------------------------------- - -// PointCloudXYZI_PointXYZI - -// required float x = 1; -inline bool PointCloudXYZI_PointXYZI::has_x() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void PointCloudXYZI_PointXYZI::set_has_x() { - _has_bits_[0] |= 0x00000001u; -} -inline void PointCloudXYZI_PointXYZI::clear_has_x() { - _has_bits_[0] &= ~0x00000001u; -} -inline void PointCloudXYZI_PointXYZI::clear_x() { - x_ = 0; - clear_has_x(); -} -inline float PointCloudXYZI_PointXYZI::x() const { - return x_; -} -inline void PointCloudXYZI_PointXYZI::set_x(float value) { - set_has_x(); - x_ = value; -} - -// required float y = 2; -inline bool PointCloudXYZI_PointXYZI::has_y() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void PointCloudXYZI_PointXYZI::set_has_y() { - _has_bits_[0] |= 0x00000002u; -} -inline void PointCloudXYZI_PointXYZI::clear_has_y() { - _has_bits_[0] &= ~0x00000002u; -} -inline void PointCloudXYZI_PointXYZI::clear_y() { - y_ = 0; - clear_has_y(); -} -inline float PointCloudXYZI_PointXYZI::y() const { - return y_; -} -inline void PointCloudXYZI_PointXYZI::set_y(float value) { - set_has_y(); - y_ = value; -} - -// required float z = 3; -inline bool PointCloudXYZI_PointXYZI::has_z() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void PointCloudXYZI_PointXYZI::set_has_z() { - _has_bits_[0] |= 0x00000004u; -} -inline void PointCloudXYZI_PointXYZI::clear_has_z() { - _has_bits_[0] &= ~0x00000004u; -} -inline void PointCloudXYZI_PointXYZI::clear_z() { - z_ = 0; - clear_has_z(); -} -inline float PointCloudXYZI_PointXYZI::z() const { - return z_; -} -inline void PointCloudXYZI_PointXYZI::set_z(float value) { - set_has_z(); - z_ = value; -} - -// required float intensity = 4; -inline bool PointCloudXYZI_PointXYZI::has_intensity() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void PointCloudXYZI_PointXYZI::set_has_intensity() { - _has_bits_[0] |= 0x00000008u; -} -inline void PointCloudXYZI_PointXYZI::clear_has_intensity() { - _has_bits_[0] &= ~0x00000008u; -} -inline void PointCloudXYZI_PointXYZI::clear_intensity() { - intensity_ = 0; - clear_has_intensity(); -} -inline float PointCloudXYZI_PointXYZI::intensity() const { - return intensity_; -} -inline void PointCloudXYZI_PointXYZI::set_intensity(float value) { - set_has_intensity(); - intensity_ = value; -} - -// ------------------------------------------------------------------- - -// PointCloudXYZI - -// required .px.HeaderInfo header = 1; -inline bool PointCloudXYZI::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void PointCloudXYZI::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void PointCloudXYZI::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void PointCloudXYZI::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& PointCloudXYZI::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* PointCloudXYZI::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* PointCloudXYZI::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// repeated .px.PointCloudXYZI.PointXYZI points = 2; -inline int PointCloudXYZI::points_size() const { - return points_.size(); -} -inline void PointCloudXYZI::clear_points() { - points_.Clear(); -} -inline const ::px::PointCloudXYZI_PointXYZI& PointCloudXYZI::points(int index) const { - return points_.Get(index); -} -inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::mutable_points(int index) { - return points_.Mutable(index); -} -inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::add_points() { - return points_.Add(); -} -inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >& -PointCloudXYZI::points() const { - return points_; -} -inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >* -PointCloudXYZI::mutable_points() { - return &points_; -} - -// ------------------------------------------------------------------- - -// PointCloudXYZRGB_PointXYZRGB - -// required float x = 1; -inline bool PointCloudXYZRGB_PointXYZRGB::has_x() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_has_x() { - _has_bits_[0] |= 0x00000001u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_has_x() { - _has_bits_[0] &= ~0x00000001u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_x() { - x_ = 0; - clear_has_x(); -} -inline float PointCloudXYZRGB_PointXYZRGB::x() const { - return x_; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_x(float value) { - set_has_x(); - x_ = value; -} - -// required float y = 2; -inline bool PointCloudXYZRGB_PointXYZRGB::has_y() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_has_y() { - _has_bits_[0] |= 0x00000002u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_has_y() { - _has_bits_[0] &= ~0x00000002u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_y() { - y_ = 0; - clear_has_y(); -} -inline float PointCloudXYZRGB_PointXYZRGB::y() const { - return y_; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_y(float value) { - set_has_y(); - y_ = value; -} - -// required float z = 3; -inline bool PointCloudXYZRGB_PointXYZRGB::has_z() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_has_z() { - _has_bits_[0] |= 0x00000004u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_has_z() { - _has_bits_[0] &= ~0x00000004u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_z() { - z_ = 0; - clear_has_z(); -} -inline float PointCloudXYZRGB_PointXYZRGB::z() const { - return z_; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_z(float value) { - set_has_z(); - z_ = value; -} - -// required float rgb = 4; -inline bool PointCloudXYZRGB_PointXYZRGB::has_rgb() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_has_rgb() { - _has_bits_[0] |= 0x00000008u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_has_rgb() { - _has_bits_[0] &= ~0x00000008u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_rgb() { - rgb_ = 0; - clear_has_rgb(); -} -inline float PointCloudXYZRGB_PointXYZRGB::rgb() const { - return rgb_; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_rgb(float value) { - set_has_rgb(); - rgb_ = value; -} - -// ------------------------------------------------------------------- - -// PointCloudXYZRGB - -// required .px.HeaderInfo header = 1; -inline bool PointCloudXYZRGB::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void PointCloudXYZRGB::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void PointCloudXYZRGB::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void PointCloudXYZRGB::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& PointCloudXYZRGB::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* PointCloudXYZRGB::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* PointCloudXYZRGB::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; -inline int PointCloudXYZRGB::points_size() const { - return points_.size(); -} -inline void PointCloudXYZRGB::clear_points() { - points_.Clear(); -} -inline const ::px::PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB::points(int index) const { - return points_.Get(index); -} -inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::mutable_points(int index) { - return points_.Mutable(index); -} -inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::add_points() { - return points_.Add(); -} -inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >& -PointCloudXYZRGB::points() const { - return points_; -} -inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >* -PointCloudXYZRGB::mutable_points() { - return &points_; -} - -// ------------------------------------------------------------------- - -// RGBDImage - -// required .px.HeaderInfo header = 1; -inline bool RGBDImage::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void RGBDImage::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void RGBDImage::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void RGBDImage::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& RGBDImage::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* RGBDImage::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* RGBDImage::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// required uint32 cols = 2; -inline bool RGBDImage::has_cols() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void RGBDImage::set_has_cols() { - _has_bits_[0] |= 0x00000002u; -} -inline void RGBDImage::clear_has_cols() { - _has_bits_[0] &= ~0x00000002u; -} -inline void RGBDImage::clear_cols() { - cols_ = 0u; - clear_has_cols(); -} -inline ::google::protobuf::uint32 RGBDImage::cols() const { - return cols_; -} -inline void RGBDImage::set_cols(::google::protobuf::uint32 value) { - set_has_cols(); - cols_ = value; -} - -// required uint32 rows = 3; -inline bool RGBDImage::has_rows() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void RGBDImage::set_has_rows() { - _has_bits_[0] |= 0x00000004u; -} -inline void RGBDImage::clear_has_rows() { - _has_bits_[0] &= ~0x00000004u; -} -inline void RGBDImage::clear_rows() { - rows_ = 0u; - clear_has_rows(); -} -inline ::google::protobuf::uint32 RGBDImage::rows() const { - return rows_; -} -inline void RGBDImage::set_rows(::google::protobuf::uint32 value) { - set_has_rows(); - rows_ = value; -} - -// required uint32 step1 = 4; -inline bool RGBDImage::has_step1() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void RGBDImage::set_has_step1() { - _has_bits_[0] |= 0x00000008u; -} -inline void RGBDImage::clear_has_step1() { - _has_bits_[0] &= ~0x00000008u; -} -inline void RGBDImage::clear_step1() { - step1_ = 0u; - clear_has_step1(); -} -inline ::google::protobuf::uint32 RGBDImage::step1() const { - return step1_; -} -inline void RGBDImage::set_step1(::google::protobuf::uint32 value) { - set_has_step1(); - step1_ = value; -} - -// required uint32 type1 = 5; -inline bool RGBDImage::has_type1() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void RGBDImage::set_has_type1() { - _has_bits_[0] |= 0x00000010u; -} -inline void RGBDImage::clear_has_type1() { - _has_bits_[0] &= ~0x00000010u; -} -inline void RGBDImage::clear_type1() { - type1_ = 0u; - clear_has_type1(); -} -inline ::google::protobuf::uint32 RGBDImage::type1() const { - return type1_; -} -inline void RGBDImage::set_type1(::google::protobuf::uint32 value) { - set_has_type1(); - type1_ = value; -} - -// required bytes imageData1 = 6; -inline bool RGBDImage::has_imagedata1() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void RGBDImage::set_has_imagedata1() { - _has_bits_[0] |= 0x00000020u; -} -inline void RGBDImage::clear_has_imagedata1() { - _has_bits_[0] &= ~0x00000020u; -} -inline void RGBDImage::clear_imagedata1() { - if (imagedata1_ != &::google::protobuf::internal::kEmptyString) { - imagedata1_->clear(); - } - clear_has_imagedata1(); -} -inline const ::std::string& RGBDImage::imagedata1() const { - return *imagedata1_; -} -inline void RGBDImage::set_imagedata1(const ::std::string& value) { - set_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - imagedata1_ = new ::std::string; - } - imagedata1_->assign(value); -} -inline void RGBDImage::set_imagedata1(const char* value) { - set_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - imagedata1_ = new ::std::string; - } - imagedata1_->assign(value); -} -inline void RGBDImage::set_imagedata1(const void* value, size_t size) { - set_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - imagedata1_ = new ::std::string; - } - imagedata1_->assign(reinterpret_cast(value), size); -} -inline ::std::string* RGBDImage::mutable_imagedata1() { - set_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - imagedata1_ = new ::std::string; - } - return imagedata1_; -} -inline ::std::string* RGBDImage::release_imagedata1() { - clear_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = imagedata1_; - imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// required uint32 step2 = 7; -inline bool RGBDImage::has_step2() const { - return (_has_bits_[0] & 0x00000040u) != 0; -} -inline void RGBDImage::set_has_step2() { - _has_bits_[0] |= 0x00000040u; -} -inline void RGBDImage::clear_has_step2() { - _has_bits_[0] &= ~0x00000040u; -} -inline void RGBDImage::clear_step2() { - step2_ = 0u; - clear_has_step2(); -} -inline ::google::protobuf::uint32 RGBDImage::step2() const { - return step2_; -} -inline void RGBDImage::set_step2(::google::protobuf::uint32 value) { - set_has_step2(); - step2_ = value; -} - -// required uint32 type2 = 8; -inline bool RGBDImage::has_type2() const { - return (_has_bits_[0] & 0x00000080u) != 0; -} -inline void RGBDImage::set_has_type2() { - _has_bits_[0] |= 0x00000080u; -} -inline void RGBDImage::clear_has_type2() { - _has_bits_[0] &= ~0x00000080u; -} -inline void RGBDImage::clear_type2() { - type2_ = 0u; - clear_has_type2(); -} -inline ::google::protobuf::uint32 RGBDImage::type2() const { - return type2_; -} -inline void RGBDImage::set_type2(::google::protobuf::uint32 value) { - set_has_type2(); - type2_ = value; -} - -// required bytes imageData2 = 9; -inline bool RGBDImage::has_imagedata2() const { - return (_has_bits_[0] & 0x00000100u) != 0; -} -inline void RGBDImage::set_has_imagedata2() { - _has_bits_[0] |= 0x00000100u; -} -inline void RGBDImage::clear_has_imagedata2() { - _has_bits_[0] &= ~0x00000100u; -} -inline void RGBDImage::clear_imagedata2() { - if (imagedata2_ != &::google::protobuf::internal::kEmptyString) { - imagedata2_->clear(); - } - clear_has_imagedata2(); -} -inline const ::std::string& RGBDImage::imagedata2() const { - return *imagedata2_; -} -inline void RGBDImage::set_imagedata2(const ::std::string& value) { - set_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - imagedata2_ = new ::std::string; - } - imagedata2_->assign(value); -} -inline void RGBDImage::set_imagedata2(const char* value) { - set_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - imagedata2_ = new ::std::string; - } - imagedata2_->assign(value); -} -inline void RGBDImage::set_imagedata2(const void* value, size_t size) { - set_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - imagedata2_ = new ::std::string; - } - imagedata2_->assign(reinterpret_cast(value), size); -} -inline ::std::string* RGBDImage::mutable_imagedata2() { - set_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - imagedata2_ = new ::std::string; - } - return imagedata2_; -} -inline ::std::string* RGBDImage::release_imagedata2() { - clear_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = imagedata2_; - imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// optional uint32 camera_config = 10; -inline bool RGBDImage::has_camera_config() const { - return (_has_bits_[0] & 0x00000200u) != 0; -} -inline void RGBDImage::set_has_camera_config() { - _has_bits_[0] |= 0x00000200u; -} -inline void RGBDImage::clear_has_camera_config() { - _has_bits_[0] &= ~0x00000200u; -} -inline void RGBDImage::clear_camera_config() { - camera_config_ = 0u; - clear_has_camera_config(); -} -inline ::google::protobuf::uint32 RGBDImage::camera_config() const { - return camera_config_; -} -inline void RGBDImage::set_camera_config(::google::protobuf::uint32 value) { - set_has_camera_config(); - camera_config_ = value; -} - -// optional uint32 camera_type = 11; -inline bool RGBDImage::has_camera_type() const { - return (_has_bits_[0] & 0x00000400u) != 0; -} -inline void RGBDImage::set_has_camera_type() { - _has_bits_[0] |= 0x00000400u; -} -inline void RGBDImage::clear_has_camera_type() { - _has_bits_[0] &= ~0x00000400u; -} -inline void RGBDImage::clear_camera_type() { - camera_type_ = 0u; - clear_has_camera_type(); -} -inline ::google::protobuf::uint32 RGBDImage::camera_type() const { - return camera_type_; -} -inline void RGBDImage::set_camera_type(::google::protobuf::uint32 value) { - set_has_camera_type(); - camera_type_ = value; -} - -// optional float roll = 12; -inline bool RGBDImage::has_roll() const { - return (_has_bits_[0] & 0x00000800u) != 0; -} -inline void RGBDImage::set_has_roll() { - _has_bits_[0] |= 0x00000800u; -} -inline void RGBDImage::clear_has_roll() { - _has_bits_[0] &= ~0x00000800u; -} -inline void RGBDImage::clear_roll() { - roll_ = 0; - clear_has_roll(); -} -inline float RGBDImage::roll() const { - return roll_; -} -inline void RGBDImage::set_roll(float value) { - set_has_roll(); - roll_ = value; -} - -// optional float pitch = 13; -inline bool RGBDImage::has_pitch() const { - return (_has_bits_[0] & 0x00001000u) != 0; -} -inline void RGBDImage::set_has_pitch() { - _has_bits_[0] |= 0x00001000u; -} -inline void RGBDImage::clear_has_pitch() { - _has_bits_[0] &= ~0x00001000u; -} -inline void RGBDImage::clear_pitch() { - pitch_ = 0; - clear_has_pitch(); -} -inline float RGBDImage::pitch() const { - return pitch_; -} -inline void RGBDImage::set_pitch(float value) { - set_has_pitch(); - pitch_ = value; -} - -// optional float yaw = 14; -inline bool RGBDImage::has_yaw() const { - return (_has_bits_[0] & 0x00002000u) != 0; -} -inline void RGBDImage::set_has_yaw() { - _has_bits_[0] |= 0x00002000u; -} -inline void RGBDImage::clear_has_yaw() { - _has_bits_[0] &= ~0x00002000u; -} -inline void RGBDImage::clear_yaw() { - yaw_ = 0; - clear_has_yaw(); -} -inline float RGBDImage::yaw() const { - return yaw_; -} -inline void RGBDImage::set_yaw(float value) { - set_has_yaw(); - yaw_ = value; -} - -// optional float lon = 15; -inline bool RGBDImage::has_lon() const { - return (_has_bits_[0] & 0x00004000u) != 0; -} -inline void RGBDImage::set_has_lon() { - _has_bits_[0] |= 0x00004000u; -} -inline void RGBDImage::clear_has_lon() { - _has_bits_[0] &= ~0x00004000u; -} -inline void RGBDImage::clear_lon() { - lon_ = 0; - clear_has_lon(); -} -inline float RGBDImage::lon() const { - return lon_; -} -inline void RGBDImage::set_lon(float value) { - set_has_lon(); - lon_ = value; -} - -// optional float lat = 16; -inline bool RGBDImage::has_lat() const { - return (_has_bits_[0] & 0x00008000u) != 0; -} -inline void RGBDImage::set_has_lat() { - _has_bits_[0] |= 0x00008000u; -} -inline void RGBDImage::clear_has_lat() { - _has_bits_[0] &= ~0x00008000u; -} -inline void RGBDImage::clear_lat() { - lat_ = 0; - clear_has_lat(); -} -inline float RGBDImage::lat() const { - return lat_; -} -inline void RGBDImage::set_lat(float value) { - set_has_lat(); - lat_ = value; -} - -// optional float alt = 17; -inline bool RGBDImage::has_alt() const { - return (_has_bits_[0] & 0x00010000u) != 0; -} -inline void RGBDImage::set_has_alt() { - _has_bits_[0] |= 0x00010000u; -} -inline void RGBDImage::clear_has_alt() { - _has_bits_[0] &= ~0x00010000u; -} -inline void RGBDImage::clear_alt() { - alt_ = 0; - clear_has_alt(); -} -inline float RGBDImage::alt() const { - return alt_; -} -inline void RGBDImage::set_alt(float value) { - set_has_alt(); - alt_ = value; -} - -// optional float ground_x = 18; -inline bool RGBDImage::has_ground_x() const { - return (_has_bits_[0] & 0x00020000u) != 0; -} -inline void RGBDImage::set_has_ground_x() { - _has_bits_[0] |= 0x00020000u; -} -inline void RGBDImage::clear_has_ground_x() { - _has_bits_[0] &= ~0x00020000u; -} -inline void RGBDImage::clear_ground_x() { - ground_x_ = 0; - clear_has_ground_x(); -} -inline float RGBDImage::ground_x() const { - return ground_x_; -} -inline void RGBDImage::set_ground_x(float value) { - set_has_ground_x(); - ground_x_ = value; -} - -// optional float ground_y = 19; -inline bool RGBDImage::has_ground_y() const { - return (_has_bits_[0] & 0x00040000u) != 0; -} -inline void RGBDImage::set_has_ground_y() { - _has_bits_[0] |= 0x00040000u; -} -inline void RGBDImage::clear_has_ground_y() { - _has_bits_[0] &= ~0x00040000u; -} -inline void RGBDImage::clear_ground_y() { - ground_y_ = 0; - clear_has_ground_y(); -} -inline float RGBDImage::ground_y() const { - return ground_y_; -} -inline void RGBDImage::set_ground_y(float value) { - set_has_ground_y(); - ground_y_ = value; -} - -// optional float ground_z = 20; -inline bool RGBDImage::has_ground_z() const { - return (_has_bits_[0] & 0x00080000u) != 0; -} -inline void RGBDImage::set_has_ground_z() { - _has_bits_[0] |= 0x00080000u; -} -inline void RGBDImage::clear_has_ground_z() { - _has_bits_[0] &= ~0x00080000u; -} -inline void RGBDImage::clear_ground_z() { - ground_z_ = 0; - clear_has_ground_z(); -} -inline float RGBDImage::ground_z() const { - return ground_z_; -} -inline void RGBDImage::set_ground_z(float value) { - set_has_ground_z(); - ground_z_ = value; -} - -// repeated float camera_matrix = 21; -inline int RGBDImage::camera_matrix_size() const { - return camera_matrix_.size(); -} -inline void RGBDImage::clear_camera_matrix() { - camera_matrix_.Clear(); -} -inline float RGBDImage::camera_matrix(int index) const { - return camera_matrix_.Get(index); -} -inline void RGBDImage::set_camera_matrix(int index, float value) { - camera_matrix_.Set(index, value); -} -inline void RGBDImage::add_camera_matrix(float value) { - camera_matrix_.Add(value); -} -inline const ::google::protobuf::RepeatedField< float >& -RGBDImage::camera_matrix() const { - return camera_matrix_; -} -inline ::google::protobuf::RepeatedField< float >* -RGBDImage::mutable_camera_matrix() { - return &camera_matrix_; -} - -// ------------------------------------------------------------------- - -// Waypoint - -// required double x = 1; -inline bool Waypoint::has_x() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void Waypoint::set_has_x() { - _has_bits_[0] |= 0x00000001u; -} -inline void Waypoint::clear_has_x() { - _has_bits_[0] &= ~0x00000001u; -} -inline void Waypoint::clear_x() { - x_ = 0; - clear_has_x(); -} -inline double Waypoint::x() const { - return x_; -} -inline void Waypoint::set_x(double value) { - set_has_x(); - x_ = value; -} - -// required double y = 2; -inline bool Waypoint::has_y() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void Waypoint::set_has_y() { - _has_bits_[0] |= 0x00000002u; -} -inline void Waypoint::clear_has_y() { - _has_bits_[0] &= ~0x00000002u; -} -inline void Waypoint::clear_y() { - y_ = 0; - clear_has_y(); -} -inline double Waypoint::y() const { - return y_; -} -inline void Waypoint::set_y(double value) { - set_has_y(); - y_ = value; -} - -// optional double z = 3; -inline bool Waypoint::has_z() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void Waypoint::set_has_z() { - _has_bits_[0] |= 0x00000004u; -} -inline void Waypoint::clear_has_z() { - _has_bits_[0] &= ~0x00000004u; -} -inline void Waypoint::clear_z() { - z_ = 0; - clear_has_z(); -} -inline double Waypoint::z() const { - return z_; -} -inline void Waypoint::set_z(double value) { - set_has_z(); - z_ = value; -} - -// optional double roll = 4; -inline bool Waypoint::has_roll() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void Waypoint::set_has_roll() { - _has_bits_[0] |= 0x00000008u; -} -inline void Waypoint::clear_has_roll() { - _has_bits_[0] &= ~0x00000008u; -} -inline void Waypoint::clear_roll() { - roll_ = 0; - clear_has_roll(); -} -inline double Waypoint::roll() const { - return roll_; -} -inline void Waypoint::set_roll(double value) { - set_has_roll(); - roll_ = value; -} - -// optional double pitch = 5; -inline bool Waypoint::has_pitch() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void Waypoint::set_has_pitch() { - _has_bits_[0] |= 0x00000010u; -} -inline void Waypoint::clear_has_pitch() { - _has_bits_[0] &= ~0x00000010u; -} -inline void Waypoint::clear_pitch() { - pitch_ = 0; - clear_has_pitch(); -} -inline double Waypoint::pitch() const { - return pitch_; -} -inline void Waypoint::set_pitch(double value) { - set_has_pitch(); - pitch_ = value; -} - -// optional double yaw = 6; -inline bool Waypoint::has_yaw() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void Waypoint::set_has_yaw() { - _has_bits_[0] |= 0x00000020u; -} -inline void Waypoint::clear_has_yaw() { - _has_bits_[0] &= ~0x00000020u; -} -inline void Waypoint::clear_yaw() { - yaw_ = 0; - clear_has_yaw(); -} -inline double Waypoint::yaw() const { - return yaw_; -} -inline void Waypoint::set_yaw(double value) { - set_has_yaw(); - yaw_ = value; -} - - -// @@protoc_insertion_point(namespace_scope) - -} // namespace px - -#ifndef SWIG -namespace google { -namespace protobuf { - -template <> -inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_CoordinateFrameType>() { - return ::px::GLOverlay_CoordinateFrameType_descriptor(); -} -template <> -inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_Mode>() { - return ::px::GLOverlay_Mode_descriptor(); -} -template <> -inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_Identifier>() { - return ::px::GLOverlay_Identifier_descriptor(); -} - -} // namespace google -} // namespace protobuf -#endif // SWIG - -// @@protoc_insertion_point(global_scope) - -#endif // PROTOBUF_pixhawk_2eproto__INCLUDED diff --git a/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h b/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h deleted file mode 100644 index caa4d3dcb..000000000 --- a/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h +++ /dev/null @@ -1,996 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from pixhawk.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef PIXHAWK_TESTSUITE_H -#define PIXHAWK_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_pixhawk(uint8_t, uint8_t, mavlink_message_t *last_msg); - -static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_test_common(system_id, component_id, last_msg); - mavlink_test_pixhawk(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_set_cam_shutter(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_set_cam_shutter_t packet_in = { - 17.0, - }17443, - }17547, - }29, - }96, - }163, - }; - mavlink_set_cam_shutter_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.gain = packet_in.gain; - packet1.interval = packet_in.interval; - packet1.exposure = packet_in.exposure; - packet1.cam_no = packet_in.cam_no; - packet1.cam_mode = packet_in.cam_mode; - packet1.trigger_pin = packet_in.trigger_pin; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_cam_shutter_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_set_cam_shutter_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_cam_shutter_pack(system_id, component_id, &msg , packet1.cam_no , packet1.cam_mode , packet1.trigger_pin , packet1.interval , packet1.exposure , packet1.gain ); - mavlink_msg_set_cam_shutter_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_cam_shutter_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.cam_no , packet1.cam_mode , packet1.trigger_pin , packet1.interval , packet1.exposure , packet1.gain ); - mavlink_msg_set_cam_shutter_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; ilen + MAVLINK_NUM_NON_PAYLOAD_BYTES; -} - -#if MAVLINK_NEED_BYTE_SWAP -static inline void byte_swap_2(char *dst, const char *src) -{ - dst[0] = src[1]; - dst[1] = src[0]; -} -static inline void byte_swap_4(char *dst, const char *src) -{ - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; -} -static inline void byte_swap_8(char *dst, const char *src) -{ - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; -} -#elif !MAVLINK_ALIGNED_FIELDS -static inline void byte_copy_2(char *dst, const char *src) -{ - dst[0] = src[0]; - dst[1] = src[1]; -} -static inline void byte_copy_4(char *dst, const char *src) -{ - dst[0] = src[0]; - dst[1] = src[1]; - dst[2] = src[2]; - dst[3] = src[3]; -} -static inline void byte_copy_8(char *dst, const char *src) -{ - memcpy(dst, src, 8); -} -#endif - -#define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b -#define _mav_put_int8_t(buf, wire_offset, b) buf[wire_offset] = (int8_t)b -#define _mav_put_char(buf, wire_offset, b) buf[wire_offset] = b - -#if MAVLINK_NEED_BYTE_SWAP -#define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b) -#define _mav_put_int16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b) -#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_int32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) -#define _mav_put_int64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) -#define _mav_put_float(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_double(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) -#elif !MAVLINK_ALIGNED_FIELDS -#define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b) -#define _mav_put_int16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b) -#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_int32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) -#define _mav_put_int64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) -#define _mav_put_float(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_double(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) -#else -#define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b -#define _mav_put_int16_t(buf, wire_offset, b) *(int16_t *)&buf[wire_offset] = b -#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b -#define _mav_put_int32_t(buf, wire_offset, b) *(int32_t *)&buf[wire_offset] = b -#define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b -#define _mav_put_int64_t(buf, wire_offset, b) *(int64_t *)&buf[wire_offset] = b -#define _mav_put_float(buf, wire_offset, b) *(float *)&buf[wire_offset] = b -#define _mav_put_double(buf, wire_offset, b) *(double *)&buf[wire_offset] = b -#endif - -/* - like memcpy(), but if src is NULL, do a memset to zero -*/ -static inline void mav_array_memcpy(void *dest, const void *src, size_t n) -{ - if (src == NULL) { - memset(dest, 0, n); - } else { - memcpy(dest, src, n); - } -} - -/* - * Place a char array into a buffer - */ -static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length) -{ - mav_array_memcpy(&buf[wire_offset], b, array_length); - -} - -/* - * Place a uint8_t array into a buffer - */ -static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length) -{ - mav_array_memcpy(&buf[wire_offset], b, array_length); - -} - -/* - * Place a int8_t array into a buffer - */ -static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length) -{ - mav_array_memcpy(&buf[wire_offset], b, array_length); - -} - -#if MAVLINK_NEED_BYTE_SWAP -#define _MAV_PUT_ARRAY(TYPE, V) \ -static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ -{ \ - if (b == NULL) { \ - memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \ - } else { \ - uint16_t i; \ - for (i=0; imsgid = MAVLINK_MSG_ID_CMD_AIRSPEED_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif -} - -/** - * @brief Pack a cmd_airspeed_ack message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param spCmd - - - * @param ack - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float spCmd,uint8_t ack) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, ack); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#else - mavlink_cmd_airspeed_ack_t packet; - packet.spCmd = spCmd; - packet.ack = ack; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif -} - -/** - * @brief Encode a cmd_airspeed_ack struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param cmd_airspeed_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) -{ - return mavlink_msg_cmd_airspeed_ack_pack(system_id, component_id, msg, cmd_airspeed_ack->spCmd, cmd_airspeed_ack->ack); -} - -/** - * @brief Encode a cmd_airspeed_ack struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param cmd_airspeed_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) -{ - return mavlink_msg_cmd_airspeed_ack_pack_chan(system_id, component_id, chan, msg, cmd_airspeed_ack->spCmd, cmd_airspeed_ack->ack); -} - -/** - * @brief Send a cmd_airspeed_ack message - * @param chan MAVLink channel to send the message - * - * @param spCmd - - - * @param ack - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_cmd_airspeed_ack_send(mavlink_channel_t chan, float spCmd, uint8_t ack) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, ack); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif -#else - mavlink_cmd_airspeed_ack_t packet; - packet.spCmd = spCmd; - packet.ack = ack; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)&packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)&packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_cmd_airspeed_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float spCmd, uint8_t ack) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, ack); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif -#else - mavlink_cmd_airspeed_ack_t *packet = (mavlink_cmd_airspeed_ack_t *)msgbuf; - packet->spCmd = spCmd; - packet->ack = ack; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE CMD_AIRSPEED_ACK UNPACKING - - -/** - * @brief Get field spCmd from cmd_airspeed_ack message - * - * @return - - - */ -static inline float mavlink_msg_cmd_airspeed_ack_get_spCmd(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field ack from cmd_airspeed_ack message - * - * @return - - - */ -static inline uint8_t mavlink_msg_cmd_airspeed_ack_get_ack(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Decode a cmd_airspeed_ack message into a struct - * - * @param msg The message to decode - * @param cmd_airspeed_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_cmd_airspeed_ack_decode(const mavlink_message_t* msg, mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - cmd_airspeed_ack->spCmd = mavlink_msg_cmd_airspeed_ack_get_spCmd(msg); - cmd_airspeed_ack->ack = mavlink_msg_cmd_airspeed_ack_get_ack(msg); -#else - memcpy(cmd_airspeed_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h deleted file mode 100644 index 71d61e1e0..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h +++ /dev/null @@ -1,253 +0,0 @@ -// MESSAGE CMD_AIRSPEED_CHNG PACKING - -#define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG 192 - -typedef struct __mavlink_cmd_airspeed_chng_t -{ - float spCmd; ///< - - - uint8_t target; ///< - - -} mavlink_cmd_airspeed_chng_t; - -#define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN 5 -#define MAVLINK_MSG_ID_192_LEN 5 - -#define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC 209 -#define MAVLINK_MSG_ID_192_CRC 209 - - - -#define MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG { \ - "CMD_AIRSPEED_CHNG", \ - 2, \ - { { "spCmd", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_cmd_airspeed_chng_t, spCmd) }, \ - { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_cmd_airspeed_chng_t, target) }, \ - } \ -} - - -/** - * @brief Pack a cmd_airspeed_chng message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target - - - * @param spCmd - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, float spCmd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, target); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#else - mavlink_cmd_airspeed_chng_t packet; - packet.spCmd = spCmd; - packet.target = target; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif -} - -/** - * @brief Pack a cmd_airspeed_chng message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target - - - * @param spCmd - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,float spCmd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, target); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#else - mavlink_cmd_airspeed_chng_t packet; - packet.spCmd = spCmd; - packet.target = target; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif -} - -/** - * @brief Encode a cmd_airspeed_chng struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param cmd_airspeed_chng C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) -{ - return mavlink_msg_cmd_airspeed_chng_pack(system_id, component_id, msg, cmd_airspeed_chng->target, cmd_airspeed_chng->spCmd); -} - -/** - * @brief Encode a cmd_airspeed_chng struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param cmd_airspeed_chng C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) -{ - return mavlink_msg_cmd_airspeed_chng_pack_chan(system_id, component_id, chan, msg, cmd_airspeed_chng->target, cmd_airspeed_chng->spCmd); -} - -/** - * @brief Send a cmd_airspeed_chng message - * @param chan MAVLink channel to send the message - * - * @param target - - - * @param spCmd - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_cmd_airspeed_chng_send(mavlink_channel_t chan, uint8_t target, float spCmd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, target); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif -#else - mavlink_cmd_airspeed_chng_t packet; - packet.spCmd = spCmd; - packet.target = target; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)&packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)&packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_cmd_airspeed_chng_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, float spCmd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, target); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif -#else - mavlink_cmd_airspeed_chng_t *packet = (mavlink_cmd_airspeed_chng_t *)msgbuf; - packet->spCmd = spCmd; - packet->target = target; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE CMD_AIRSPEED_CHNG UNPACKING - - -/** - * @brief Get field target from cmd_airspeed_chng message - * - * @return - - - */ -static inline uint8_t mavlink_msg_cmd_airspeed_chng_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field spCmd from cmd_airspeed_chng message - * - * @return - - - */ -static inline float mavlink_msg_cmd_airspeed_chng_get_spCmd(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a cmd_airspeed_chng message into a struct - * - * @param msg The message to decode - * @param cmd_airspeed_chng C-struct to decode the message contents into - */ -static inline void mavlink_msg_cmd_airspeed_chng_decode(const mavlink_message_t* msg, mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) -{ -#if MAVLINK_NEED_BYTE_SWAP - cmd_airspeed_chng->spCmd = mavlink_msg_cmd_airspeed_chng_get_spCmd(msg); - cmd_airspeed_chng->target = mavlink_msg_cmd_airspeed_chng_get_target(msg); -#else - memcpy(cmd_airspeed_chng, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h deleted file mode 100644 index 40a3db25a..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h +++ /dev/null @@ -1,219 +0,0 @@ -// MESSAGE FILT_ROT_VEL PACKING - -#define MAVLINK_MSG_ID_FILT_ROT_VEL 184 - -typedef struct __mavlink_filt_rot_vel_t -{ - float rotVel[3]; ///< - - -} mavlink_filt_rot_vel_t; - -#define MAVLINK_MSG_ID_FILT_ROT_VEL_LEN 12 -#define MAVLINK_MSG_ID_184_LEN 12 - -#define MAVLINK_MSG_ID_FILT_ROT_VEL_CRC 79 -#define MAVLINK_MSG_ID_184_CRC 79 - -#define MAVLINK_MSG_FILT_ROT_VEL_FIELD_ROTVEL_LEN 3 - -#define MAVLINK_MESSAGE_INFO_FILT_ROT_VEL { \ - "FILT_ROT_VEL", \ - 1, \ - { { "rotVel", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_filt_rot_vel_t, rotVel) }, \ - } \ -} - - -/** - * @brief Pack a filt_rot_vel message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param rotVel - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_filt_rot_vel_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const float *rotVel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILT_ROT_VEL_LEN]; - - _mav_put_float_array(buf, 0, rotVel, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#else - mavlink_filt_rot_vel_t packet; - - mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILT_ROT_VEL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif -} - -/** - * @brief Pack a filt_rot_vel message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param rotVel - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_filt_rot_vel_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const float *rotVel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILT_ROT_VEL_LEN]; - - _mav_put_float_array(buf, 0, rotVel, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#else - mavlink_filt_rot_vel_t packet; - - mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILT_ROT_VEL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif -} - -/** - * @brief Encode a filt_rot_vel struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param filt_rot_vel C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_filt_rot_vel_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_filt_rot_vel_t* filt_rot_vel) -{ - return mavlink_msg_filt_rot_vel_pack(system_id, component_id, msg, filt_rot_vel->rotVel); -} - -/** - * @brief Encode a filt_rot_vel struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param filt_rot_vel C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_filt_rot_vel_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_filt_rot_vel_t* filt_rot_vel) -{ - return mavlink_msg_filt_rot_vel_pack_chan(system_id, component_id, chan, msg, filt_rot_vel->rotVel); -} - -/** - * @brief Send a filt_rot_vel message - * @param chan MAVLink channel to send the message - * - * @param rotVel - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_filt_rot_vel_send(mavlink_channel_t chan, const float *rotVel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_FILT_ROT_VEL_LEN]; - - _mav_put_float_array(buf, 0, rotVel, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif -#else - mavlink_filt_rot_vel_t packet; - - mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)&packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)&packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_FILT_ROT_VEL_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_filt_rot_vel_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const float *rotVel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - - _mav_put_float_array(buf, 0, rotVel, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif -#else - mavlink_filt_rot_vel_t *packet = (mavlink_filt_rot_vel_t *)msgbuf; - - mav_array_memcpy(packet->rotVel, rotVel, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE FILT_ROT_VEL UNPACKING - - -/** - * @brief Get field rotVel from filt_rot_vel message - * - * @return - - - */ -static inline uint16_t mavlink_msg_filt_rot_vel_get_rotVel(const mavlink_message_t* msg, float *rotVel) -{ - return _MAV_RETURN_float_array(msg, rotVel, 3, 0); -} - -/** - * @brief Decode a filt_rot_vel message into a struct - * - * @param msg The message to decode - * @param filt_rot_vel C-struct to decode the message contents into - */ -static inline void mavlink_msg_filt_rot_vel_decode(const mavlink_message_t* msg, mavlink_filt_rot_vel_t* filt_rot_vel) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_filt_rot_vel_get_rotVel(msg, filt_rot_vel->rotVel); -#else - memcpy(filt_rot_vel, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h deleted file mode 100644 index bceed1013..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h +++ /dev/null @@ -1,254 +0,0 @@ -// MESSAGE LLC_OUT PACKING - -#define MAVLINK_MSG_ID_LLC_OUT 186 - -typedef struct __mavlink_llc_out_t -{ - int16_t servoOut[4]; ///< - - - int16_t MotorOut[2]; ///< - - -} mavlink_llc_out_t; - -#define MAVLINK_MSG_ID_LLC_OUT_LEN 12 -#define MAVLINK_MSG_ID_186_LEN 12 - -#define MAVLINK_MSG_ID_LLC_OUT_CRC 5 -#define MAVLINK_MSG_ID_186_CRC 5 - -#define MAVLINK_MSG_LLC_OUT_FIELD_SERVOOUT_LEN 4 -#define MAVLINK_MSG_LLC_OUT_FIELD_MOTOROUT_LEN 2 - -#define MAVLINK_MESSAGE_INFO_LLC_OUT { \ - "LLC_OUT", \ - 2, \ - { { "servoOut", NULL, MAVLINK_TYPE_INT16_T, 4, 0, offsetof(mavlink_llc_out_t, servoOut) }, \ - { "MotorOut", NULL, MAVLINK_TYPE_INT16_T, 2, 8, offsetof(mavlink_llc_out_t, MotorOut) }, \ - } \ -} - - -/** - * @brief Pack a llc_out message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param servoOut - - - * @param MotorOut - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_llc_out_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const int16_t *servoOut, const int16_t *MotorOut) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LLC_OUT_LEN]; - - _mav_put_int16_t_array(buf, 0, servoOut, 4); - _mav_put_int16_t_array(buf, 8, MotorOut, 2); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LLC_OUT_LEN); -#else - mavlink_llc_out_t packet; - - mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4); - mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LLC_OUT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif -} - -/** - * @brief Pack a llc_out message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param servoOut - - - * @param MotorOut - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_llc_out_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const int16_t *servoOut,const int16_t *MotorOut) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LLC_OUT_LEN]; - - _mav_put_int16_t_array(buf, 0, servoOut, 4); - _mav_put_int16_t_array(buf, 8, MotorOut, 2); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LLC_OUT_LEN); -#else - mavlink_llc_out_t packet; - - mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4); - mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_LLC_OUT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif -} - -/** - * @brief Encode a llc_out struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param llc_out C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_llc_out_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_llc_out_t* llc_out) -{ - return mavlink_msg_llc_out_pack(system_id, component_id, msg, llc_out->servoOut, llc_out->MotorOut); -} - -/** - * @brief Encode a llc_out struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param llc_out C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_llc_out_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_llc_out_t* llc_out) -{ - return mavlink_msg_llc_out_pack_chan(system_id, component_id, chan, msg, llc_out->servoOut, llc_out->MotorOut); -} - -/** - * @brief Send a llc_out message - * @param chan MAVLink channel to send the message - * - * @param servoOut - - - * @param MotorOut - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_llc_out_send(mavlink_channel_t chan, const int16_t *servoOut, const int16_t *MotorOut) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_LLC_OUT_LEN]; - - _mav_put_int16_t_array(buf, 0, servoOut, 4); - _mav_put_int16_t_array(buf, 8, MotorOut, 2); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif -#else - mavlink_llc_out_t packet; - - mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4); - mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)&packet, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)&packet, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_LLC_OUT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_llc_out_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const int16_t *servoOut, const int16_t *MotorOut) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - - _mav_put_int16_t_array(buf, 0, servoOut, 4); - _mav_put_int16_t_array(buf, 8, MotorOut, 2); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif -#else - mavlink_llc_out_t *packet = (mavlink_llc_out_t *)msgbuf; - - mav_array_memcpy(packet->servoOut, servoOut, sizeof(int16_t)*4); - mav_array_memcpy(packet->MotorOut, MotorOut, sizeof(int16_t)*2); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)packet, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)packet, MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE LLC_OUT UNPACKING - - -/** - * @brief Get field servoOut from llc_out message - * - * @return - - - */ -static inline uint16_t mavlink_msg_llc_out_get_servoOut(const mavlink_message_t* msg, int16_t *servoOut) -{ - return _MAV_RETURN_int16_t_array(msg, servoOut, 4, 0); -} - -/** - * @brief Get field MotorOut from llc_out message - * - * @return - - - */ -static inline uint16_t mavlink_msg_llc_out_get_MotorOut(const mavlink_message_t* msg, int16_t *MotorOut) -{ - return _MAV_RETURN_int16_t_array(msg, MotorOut, 2, 8); -} - -/** - * @brief Decode a llc_out message into a struct - * - * @param msg The message to decode - * @param llc_out C-struct to decode the message contents into - */ -static inline void mavlink_msg_llc_out_decode(const mavlink_message_t* msg, mavlink_llc_out_t* llc_out) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_llc_out_get_servoOut(msg, llc_out->servoOut); - mavlink_msg_llc_out_get_MotorOut(msg, llc_out->MotorOut); -#else - memcpy(llc_out, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LLC_OUT_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h deleted file mode 100644 index f5a43e4fa..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h +++ /dev/null @@ -1,219 +0,0 @@ -// MESSAGE OBS_AIR_TEMP PACKING - -#define MAVLINK_MSG_ID_OBS_AIR_TEMP 183 - -typedef struct __mavlink_obs_air_temp_t -{ - float airT; ///< - - -} mavlink_obs_air_temp_t; - -#define MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN 4 -#define MAVLINK_MSG_ID_183_LEN 4 - -#define MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC 248 -#define MAVLINK_MSG_ID_183_CRC 248 - - - -#define MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP { \ - "OBS_AIR_TEMP", \ - 1, \ - { { "airT", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_air_temp_t, airT) }, \ - } \ -} - - -/** - * @brief Pack a obs_air_temp message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param airT - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_air_temp_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float airT) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN]; - _mav_put_float(buf, 0, airT); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#else - mavlink_obs_air_temp_t packet; - packet.airT = airT; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif -} - -/** - * @brief Pack a obs_air_temp message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param airT - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_air_temp_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float airT) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN]; - _mav_put_float(buf, 0, airT); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#else - mavlink_obs_air_temp_t packet; - packet.airT = airT; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif -} - -/** - * @brief Encode a obs_air_temp struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_air_temp C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_air_temp_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp) -{ - return mavlink_msg_obs_air_temp_pack(system_id, component_id, msg, obs_air_temp->airT); -} - -/** - * @brief Encode a obs_air_temp struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_air_temp C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_air_temp_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp) -{ - return mavlink_msg_obs_air_temp_pack_chan(system_id, component_id, chan, msg, obs_air_temp->airT); -} - -/** - * @brief Send a obs_air_temp message - * @param chan MAVLink channel to send the message - * - * @param airT - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_air_temp_send(mavlink_channel_t chan, float airT) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN]; - _mav_put_float(buf, 0, airT); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif -#else - mavlink_obs_air_temp_t packet; - packet.airT = airT; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)&packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)&packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_obs_air_temp_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float airT) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, airT); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif -#else - mavlink_obs_air_temp_t *packet = (mavlink_obs_air_temp_t *)msgbuf; - packet->airT = airT; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE OBS_AIR_TEMP UNPACKING - - -/** - * @brief Get field airT from obs_air_temp message - * - * @return - - - */ -static inline float mavlink_msg_obs_air_temp_get_airT(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a obs_air_temp message into a struct - * - * @param msg The message to decode - * @param obs_air_temp C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_air_temp_decode(const mavlink_message_t* msg, mavlink_obs_air_temp_t* obs_air_temp) -{ -#if MAVLINK_NEED_BYTE_SWAP - obs_air_temp->airT = mavlink_msg_obs_air_temp_get_airT(msg); -#else - memcpy(obs_air_temp, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h deleted file mode 100644 index 67d5ab2a2..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE OBS_AIR_VELOCITY PACKING - -#define MAVLINK_MSG_ID_OBS_AIR_VELOCITY 178 - -typedef struct __mavlink_obs_air_velocity_t -{ - float magnitude; ///< - - - float aoa; ///< - - - float slip; ///< - - -} mavlink_obs_air_velocity_t; - -#define MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN 12 -#define MAVLINK_MSG_ID_178_LEN 12 - -#define MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC 32 -#define MAVLINK_MSG_ID_178_CRC 32 - - - -#define MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY { \ - "OBS_AIR_VELOCITY", \ - 3, \ - { { "magnitude", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_air_velocity_t, magnitude) }, \ - { "aoa", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_obs_air_velocity_t, aoa) }, \ - { "slip", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_obs_air_velocity_t, slip) }, \ - } \ -} - - -/** - * @brief Pack a obs_air_velocity message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param magnitude - - - * @param aoa - - - * @param slip - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_air_velocity_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float magnitude, float aoa, float slip) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN]; - _mav_put_float(buf, 0, magnitude); - _mav_put_float(buf, 4, aoa); - _mav_put_float(buf, 8, slip); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#else - mavlink_obs_air_velocity_t packet; - packet.magnitude = magnitude; - packet.aoa = aoa; - packet.slip = slip; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_AIR_VELOCITY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif -} - -/** - * @brief Pack a obs_air_velocity message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param magnitude - - - * @param aoa - - - * @param slip - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_air_velocity_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float magnitude,float aoa,float slip) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN]; - _mav_put_float(buf, 0, magnitude); - _mav_put_float(buf, 4, aoa); - _mav_put_float(buf, 8, slip); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#else - mavlink_obs_air_velocity_t packet; - packet.magnitude = magnitude; - packet.aoa = aoa; - packet.slip = slip; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_AIR_VELOCITY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif -} - -/** - * @brief Encode a obs_air_velocity struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_air_velocity C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_air_velocity_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_air_velocity_t* obs_air_velocity) -{ - return mavlink_msg_obs_air_velocity_pack(system_id, component_id, msg, obs_air_velocity->magnitude, obs_air_velocity->aoa, obs_air_velocity->slip); -} - -/** - * @brief Encode a obs_air_velocity struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_air_velocity C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_air_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_velocity_t* obs_air_velocity) -{ - return mavlink_msg_obs_air_velocity_pack_chan(system_id, component_id, chan, msg, obs_air_velocity->magnitude, obs_air_velocity->aoa, obs_air_velocity->slip); -} - -/** - * @brief Send a obs_air_velocity message - * @param chan MAVLink channel to send the message - * - * @param magnitude - - - * @param aoa - - - * @param slip - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_air_velocity_send(mavlink_channel_t chan, float magnitude, float aoa, float slip) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN]; - _mav_put_float(buf, 0, magnitude); - _mav_put_float(buf, 4, aoa); - _mav_put_float(buf, 8, slip); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif -#else - mavlink_obs_air_velocity_t packet; - packet.magnitude = magnitude; - packet.aoa = aoa; - packet.slip = slip; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)&packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)&packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_obs_air_velocity_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float magnitude, float aoa, float slip) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, magnitude); - _mav_put_float(buf, 4, aoa); - _mav_put_float(buf, 8, slip); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif -#else - mavlink_obs_air_velocity_t *packet = (mavlink_obs_air_velocity_t *)msgbuf; - packet->magnitude = magnitude; - packet->aoa = aoa; - packet->slip = slip; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE OBS_AIR_VELOCITY UNPACKING - - -/** - * @brief Get field magnitude from obs_air_velocity message - * - * @return - - - */ -static inline float mavlink_msg_obs_air_velocity_get_magnitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field aoa from obs_air_velocity message - * - * @return - - - */ -static inline float mavlink_msg_obs_air_velocity_get_aoa(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field slip from obs_air_velocity message - * - * @return - - - */ -static inline float mavlink_msg_obs_air_velocity_get_slip(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Decode a obs_air_velocity message into a struct - * - * @param msg The message to decode - * @param obs_air_velocity C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_air_velocity_decode(const mavlink_message_t* msg, mavlink_obs_air_velocity_t* obs_air_velocity) -{ -#if MAVLINK_NEED_BYTE_SWAP - obs_air_velocity->magnitude = mavlink_msg_obs_air_velocity_get_magnitude(msg); - obs_air_velocity->aoa = mavlink_msg_obs_air_velocity_get_aoa(msg); - obs_air_velocity->slip = mavlink_msg_obs_air_velocity_get_slip(msg); -#else - memcpy(obs_air_velocity, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h deleted file mode 100644 index e1178e25f..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h +++ /dev/null @@ -1,219 +0,0 @@ -// MESSAGE OBS_ATTITUDE PACKING - -#define MAVLINK_MSG_ID_OBS_ATTITUDE 174 - -typedef struct __mavlink_obs_attitude_t -{ - double quat[4]; ///< - - -} mavlink_obs_attitude_t; - -#define MAVLINK_MSG_ID_OBS_ATTITUDE_LEN 32 -#define MAVLINK_MSG_ID_174_LEN 32 - -#define MAVLINK_MSG_ID_OBS_ATTITUDE_CRC 146 -#define MAVLINK_MSG_ID_174_CRC 146 - -#define MAVLINK_MSG_OBS_ATTITUDE_FIELD_QUAT_LEN 4 - -#define MAVLINK_MESSAGE_INFO_OBS_ATTITUDE { \ - "OBS_ATTITUDE", \ - 1, \ - { { "quat", NULL, MAVLINK_TYPE_DOUBLE, 4, 0, offsetof(mavlink_obs_attitude_t, quat) }, \ - } \ -} - - -/** - * @brief Pack a obs_attitude message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param quat - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const double *quat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_ATTITUDE_LEN]; - - _mav_put_double_array(buf, 0, quat, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#else - mavlink_obs_attitude_t packet; - - mav_array_memcpy(packet.quat, quat, sizeof(double)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_ATTITUDE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif -} - -/** - * @brief Pack a obs_attitude message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param quat - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const double *quat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_ATTITUDE_LEN]; - - _mav_put_double_array(buf, 0, quat, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#else - mavlink_obs_attitude_t packet; - - mav_array_memcpy(packet.quat, quat, sizeof(double)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_ATTITUDE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif -} - -/** - * @brief Encode a obs_attitude struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_attitude C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_attitude_t* obs_attitude) -{ - return mavlink_msg_obs_attitude_pack(system_id, component_id, msg, obs_attitude->quat); -} - -/** - * @brief Encode a obs_attitude struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_attitude C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_attitude_t* obs_attitude) -{ - return mavlink_msg_obs_attitude_pack_chan(system_id, component_id, chan, msg, obs_attitude->quat); -} - -/** - * @brief Send a obs_attitude message - * @param chan MAVLink channel to send the message - * - * @param quat - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_attitude_send(mavlink_channel_t chan, const double *quat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_ATTITUDE_LEN]; - - _mav_put_double_array(buf, 0, quat, 4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif -#else - mavlink_obs_attitude_t packet; - - mav_array_memcpy(packet.quat, quat, sizeof(double)*4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_OBS_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_obs_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const double *quat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - - _mav_put_double_array(buf, 0, quat, 4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif -#else - mavlink_obs_attitude_t *packet = (mavlink_obs_attitude_t *)msgbuf; - - mav_array_memcpy(packet->quat, quat, sizeof(double)*4); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE OBS_ATTITUDE UNPACKING - - -/** - * @brief Get field quat from obs_attitude message - * - * @return - - - */ -static inline uint16_t mavlink_msg_obs_attitude_get_quat(const mavlink_message_t* msg, double *quat) -{ - return _MAV_RETURN_double_array(msg, quat, 4, 0); -} - -/** - * @brief Decode a obs_attitude message into a struct - * - * @param msg The message to decode - * @param obs_attitude C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_attitude_decode(const mavlink_message_t* msg, mavlink_obs_attitude_t* obs_attitude) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_obs_attitude_get_quat(msg, obs_attitude->quat); -#else - memcpy(obs_attitude, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h deleted file mode 100644 index b164687ff..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h +++ /dev/null @@ -1,254 +0,0 @@ -// MESSAGE OBS_BIAS PACKING - -#define MAVLINK_MSG_ID_OBS_BIAS 180 - -typedef struct __mavlink_obs_bias_t -{ - float accBias[3]; ///< - - - float gyroBias[3]; ///< - - -} mavlink_obs_bias_t; - -#define MAVLINK_MSG_ID_OBS_BIAS_LEN 24 -#define MAVLINK_MSG_ID_180_LEN 24 - -#define MAVLINK_MSG_ID_OBS_BIAS_CRC 159 -#define MAVLINK_MSG_ID_180_CRC 159 - -#define MAVLINK_MSG_OBS_BIAS_FIELD_ACCBIAS_LEN 3 -#define MAVLINK_MSG_OBS_BIAS_FIELD_GYROBIAS_LEN 3 - -#define MAVLINK_MESSAGE_INFO_OBS_BIAS { \ - "OBS_BIAS", \ - 2, \ - { { "accBias", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_bias_t, accBias) }, \ - { "gyroBias", NULL, MAVLINK_TYPE_FLOAT, 3, 12, offsetof(mavlink_obs_bias_t, gyroBias) }, \ - } \ -} - - -/** - * @brief Pack a obs_bias message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param accBias - - - * @param gyroBias - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const float *accBias, const float *gyroBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_BIAS_LEN]; - - _mav_put_float_array(buf, 0, accBias, 3); - _mav_put_float_array(buf, 12, gyroBias, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#else - mavlink_obs_bias_t packet; - - mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3); - mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_BIAS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif -} - -/** - * @brief Pack a obs_bias message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param accBias - - - * @param gyroBias - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const float *accBias,const float *gyroBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_BIAS_LEN]; - - _mav_put_float_array(buf, 0, accBias, 3); - _mav_put_float_array(buf, 12, gyroBias, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#else - mavlink_obs_bias_t packet; - - mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3); - mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_BIAS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif -} - -/** - * @brief Encode a obs_bias struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_bias C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_bias_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_bias_t* obs_bias) -{ - return mavlink_msg_obs_bias_pack(system_id, component_id, msg, obs_bias->accBias, obs_bias->gyroBias); -} - -/** - * @brief Encode a obs_bias struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_bias C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_bias_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_bias_t* obs_bias) -{ - return mavlink_msg_obs_bias_pack_chan(system_id, component_id, chan, msg, obs_bias->accBias, obs_bias->gyroBias); -} - -/** - * @brief Send a obs_bias message - * @param chan MAVLink channel to send the message - * - * @param accBias - - - * @param gyroBias - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_bias_send(mavlink_channel_t chan, const float *accBias, const float *gyroBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_BIAS_LEN]; - - _mav_put_float_array(buf, 0, accBias, 3); - _mav_put_float_array(buf, 12, gyroBias, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif -#else - mavlink_obs_bias_t packet; - - mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3); - mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)&packet, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)&packet, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_OBS_BIAS_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_obs_bias_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const float *accBias, const float *gyroBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - - _mav_put_float_array(buf, 0, accBias, 3); - _mav_put_float_array(buf, 12, gyroBias, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif -#else - mavlink_obs_bias_t *packet = (mavlink_obs_bias_t *)msgbuf; - - mav_array_memcpy(packet->accBias, accBias, sizeof(float)*3); - mav_array_memcpy(packet->gyroBias, gyroBias, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)packet, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)packet, MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE OBS_BIAS UNPACKING - - -/** - * @brief Get field accBias from obs_bias message - * - * @return - - - */ -static inline uint16_t mavlink_msg_obs_bias_get_accBias(const mavlink_message_t* msg, float *accBias) -{ - return _MAV_RETURN_float_array(msg, accBias, 3, 0); -} - -/** - * @brief Get field gyroBias from obs_bias message - * - * @return - - - */ -static inline uint16_t mavlink_msg_obs_bias_get_gyroBias(const mavlink_message_t* msg, float *gyroBias) -{ - return _MAV_RETURN_float_array(msg, gyroBias, 3, 12); -} - -/** - * @brief Decode a obs_bias message into a struct - * - * @param msg The message to decode - * @param obs_bias C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_bias_decode(const mavlink_message_t* msg, mavlink_obs_bias_t* obs_bias) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_obs_bias_get_accBias(msg, obs_bias->accBias); - mavlink_msg_obs_bias_get_gyroBias(msg, obs_bias->gyroBias); -#else - memcpy(obs_bias, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_BIAS_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h deleted file mode 100644 index f0ecef0e7..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h +++ /dev/null @@ -1,287 +0,0 @@ -// MESSAGE OBS_POSITION PACKING - -#define MAVLINK_MSG_ID_OBS_POSITION 170 - -typedef struct __mavlink_obs_position_t -{ - int32_t lon; ///< - - - int32_t lat; ///< - - - int32_t alt; ///< - - -} mavlink_obs_position_t; - -#define MAVLINK_MSG_ID_OBS_POSITION_LEN 12 -#define MAVLINK_MSG_ID_170_LEN 12 - -#define MAVLINK_MSG_ID_OBS_POSITION_CRC 15 -#define MAVLINK_MSG_ID_170_CRC 15 - - - -#define MAVLINK_MESSAGE_INFO_OBS_POSITION { \ - "OBS_POSITION", \ - 3, \ - { { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_obs_position_t, lon) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_obs_position_t, lat) }, \ - { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_obs_position_t, alt) }, \ - } \ -} - - -/** - * @brief Pack a obs_position message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param lon - - - * @param lat - - - * @param alt - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int32_t lon, int32_t lat, int32_t alt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_POSITION_LEN]; - _mav_put_int32_t(buf, 0, lon); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, alt); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#else - mavlink_obs_position_t packet; - packet.lon = lon; - packet.lat = lat; - packet.alt = alt; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_POSITION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif -} - -/** - * @brief Pack a obs_position message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param lon - - - * @param lat - - - * @param alt - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - int32_t lon,int32_t lat,int32_t alt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_POSITION_LEN]; - _mav_put_int32_t(buf, 0, lon); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, alt); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#else - mavlink_obs_position_t packet; - packet.lon = lon; - packet.lat = lat; - packet.alt = alt; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_POSITION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif -} - -/** - * @brief Encode a obs_position struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_position C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_position_t* obs_position) -{ - return mavlink_msg_obs_position_pack(system_id, component_id, msg, obs_position->lon, obs_position->lat, obs_position->alt); -} - -/** - * @brief Encode a obs_position struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_position C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_position_t* obs_position) -{ - return mavlink_msg_obs_position_pack_chan(system_id, component_id, chan, msg, obs_position->lon, obs_position->lat, obs_position->alt); -} - -/** - * @brief Send a obs_position message - * @param chan MAVLink channel to send the message - * - * @param lon - - - * @param lat - - - * @param alt - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_position_send(mavlink_channel_t chan, int32_t lon, int32_t lat, int32_t alt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_POSITION_LEN]; - _mav_put_int32_t(buf, 0, lon); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, alt); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif -#else - mavlink_obs_position_t packet; - packet.lon = lon; - packet.lat = lat; - packet.alt = alt; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)&packet, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)&packet, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_OBS_POSITION_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_obs_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lon, int32_t lat, int32_t alt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int32_t(buf, 0, lon); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, alt); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif -#else - mavlink_obs_position_t *packet = (mavlink_obs_position_t *)msgbuf; - packet->lon = lon; - packet->lat = lat; - packet->alt = alt; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)packet, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)packet, MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE OBS_POSITION UNPACKING - - -/** - * @brief Get field lon from obs_position message - * - * @return - - - */ -static inline int32_t mavlink_msg_obs_position_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field lat from obs_position message - * - * @return - - - */ -static inline int32_t mavlink_msg_obs_position_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field alt from obs_position message - * - * @return - - - */ -static inline int32_t mavlink_msg_obs_position_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Decode a obs_position message into a struct - * - * @param msg The message to decode - * @param obs_position C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_position_decode(const mavlink_message_t* msg, mavlink_obs_position_t* obs_position) -{ -#if MAVLINK_NEED_BYTE_SWAP - obs_position->lon = mavlink_msg_obs_position_get_lon(msg); - obs_position->lat = mavlink_msg_obs_position_get_lat(msg); - obs_position->alt = mavlink_msg_obs_position_get_alt(msg); -#else - memcpy(obs_position, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_POSITION_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h deleted file mode 100644 index 31617c883..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h +++ /dev/null @@ -1,219 +0,0 @@ -// MESSAGE OBS_QFF PACKING - -#define MAVLINK_MSG_ID_OBS_QFF 182 - -typedef struct __mavlink_obs_qff_t -{ - float qff; ///< - - -} mavlink_obs_qff_t; - -#define MAVLINK_MSG_ID_OBS_QFF_LEN 4 -#define MAVLINK_MSG_ID_182_LEN 4 - -#define MAVLINK_MSG_ID_OBS_QFF_CRC 24 -#define MAVLINK_MSG_ID_182_CRC 24 - - - -#define MAVLINK_MESSAGE_INFO_OBS_QFF { \ - "OBS_QFF", \ - 1, \ - { { "qff", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_qff_t, qff) }, \ - } \ -} - - -/** - * @brief Pack a obs_qff message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param qff - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_qff_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float qff) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_QFF_LEN]; - _mav_put_float(buf, 0, qff); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_QFF_LEN); -#else - mavlink_obs_qff_t packet; - packet.qff = qff; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_QFF; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif -} - -/** - * @brief Pack a obs_qff message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param qff - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_qff_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float qff) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_QFF_LEN]; - _mav_put_float(buf, 0, qff); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_QFF_LEN); -#else - mavlink_obs_qff_t packet; - packet.qff = qff; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_QFF; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif -} - -/** - * @brief Encode a obs_qff struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_qff C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_qff_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff) -{ - return mavlink_msg_obs_qff_pack(system_id, component_id, msg, obs_qff->qff); -} - -/** - * @brief Encode a obs_qff struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_qff C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_qff_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff) -{ - return mavlink_msg_obs_qff_pack_chan(system_id, component_id, chan, msg, obs_qff->qff); -} - -/** - * @brief Send a obs_qff message - * @param chan MAVLink channel to send the message - * - * @param qff - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_qff_send(mavlink_channel_t chan, float qff) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_QFF_LEN]; - _mav_put_float(buf, 0, qff); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif -#else - mavlink_obs_qff_t packet; - packet.qff = qff; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)&packet, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)&packet, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_OBS_QFF_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_obs_qff_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float qff) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, qff); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif -#else - mavlink_obs_qff_t *packet = (mavlink_obs_qff_t *)msgbuf; - packet->qff = qff; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)packet, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)packet, MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE OBS_QFF UNPACKING - - -/** - * @brief Get field qff from obs_qff message - * - * @return - - - */ -static inline float mavlink_msg_obs_qff_get_qff(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a obs_qff message into a struct - * - * @param msg The message to decode - * @param obs_qff C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_qff_decode(const mavlink_message_t* msg, mavlink_obs_qff_t* obs_qff) -{ -#if MAVLINK_NEED_BYTE_SWAP - obs_qff->qff = mavlink_msg_obs_qff_get_qff(msg); -#else - memcpy(obs_qff, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_QFF_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h deleted file mode 100644 index ec815d692..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h +++ /dev/null @@ -1,219 +0,0 @@ -// MESSAGE OBS_VELOCITY PACKING - -#define MAVLINK_MSG_ID_OBS_VELOCITY 172 - -typedef struct __mavlink_obs_velocity_t -{ - float vel[3]; ///< - - -} mavlink_obs_velocity_t; - -#define MAVLINK_MSG_ID_OBS_VELOCITY_LEN 12 -#define MAVLINK_MSG_ID_172_LEN 12 - -#define MAVLINK_MSG_ID_OBS_VELOCITY_CRC 108 -#define MAVLINK_MSG_ID_172_CRC 108 - -#define MAVLINK_MSG_OBS_VELOCITY_FIELD_VEL_LEN 3 - -#define MAVLINK_MESSAGE_INFO_OBS_VELOCITY { \ - "OBS_VELOCITY", \ - 1, \ - { { "vel", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_velocity_t, vel) }, \ - } \ -} - - -/** - * @brief Pack a obs_velocity message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param vel - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_velocity_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const float *vel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN]; - - _mav_put_float_array(buf, 0, vel, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#else - mavlink_obs_velocity_t packet; - - mav_array_memcpy(packet.vel, vel, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif -} - -/** - * @brief Pack a obs_velocity message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param vel - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_velocity_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const float *vel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN]; - - _mav_put_float_array(buf, 0, vel, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#else - mavlink_obs_velocity_t packet; - - mav_array_memcpy(packet.vel, vel, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif -} - -/** - * @brief Encode a obs_velocity struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_velocity C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_velocity_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity) -{ - return mavlink_msg_obs_velocity_pack(system_id, component_id, msg, obs_velocity->vel); -} - -/** - * @brief Encode a obs_velocity struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_velocity C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity) -{ - return mavlink_msg_obs_velocity_pack_chan(system_id, component_id, chan, msg, obs_velocity->vel); -} - -/** - * @brief Send a obs_velocity message - * @param chan MAVLink channel to send the message - * - * @param vel - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_velocity_send(mavlink_channel_t chan, const float *vel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN]; - - _mav_put_float_array(buf, 0, vel, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif -#else - mavlink_obs_velocity_t packet; - - mav_array_memcpy(packet.vel, vel, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)&packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)&packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_OBS_VELOCITY_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_obs_velocity_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const float *vel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - - _mav_put_float_array(buf, 0, vel, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif -#else - mavlink_obs_velocity_t *packet = (mavlink_obs_velocity_t *)msgbuf; - - mav_array_memcpy(packet->vel, vel, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE OBS_VELOCITY UNPACKING - - -/** - * @brief Get field vel from obs_velocity message - * - * @return - - - */ -static inline uint16_t mavlink_msg_obs_velocity_get_vel(const mavlink_message_t* msg, float *vel) -{ - return _MAV_RETURN_float_array(msg, vel, 3, 0); -} - -/** - * @brief Decode a obs_velocity message into a struct - * - * @param msg The message to decode - * @param obs_velocity C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_velocity_decode(const mavlink_message_t* msg, mavlink_obs_velocity_t* obs_velocity) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_obs_velocity_get_vel(msg, obs_velocity->vel); -#else - memcpy(obs_velocity, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_VELOCITY_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h deleted file mode 100644 index c40391d44..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h +++ /dev/null @@ -1,219 +0,0 @@ -// MESSAGE OBS_WIND PACKING - -#define MAVLINK_MSG_ID_OBS_WIND 176 - -typedef struct __mavlink_obs_wind_t -{ - float wind[3]; ///< - - -} mavlink_obs_wind_t; - -#define MAVLINK_MSG_ID_OBS_WIND_LEN 12 -#define MAVLINK_MSG_ID_176_LEN 12 - -#define MAVLINK_MSG_ID_OBS_WIND_CRC 16 -#define MAVLINK_MSG_ID_176_CRC 16 - -#define MAVLINK_MSG_OBS_WIND_FIELD_WIND_LEN 3 - -#define MAVLINK_MESSAGE_INFO_OBS_WIND { \ - "OBS_WIND", \ - 1, \ - { { "wind", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_wind_t, wind) }, \ - } \ -} - - -/** - * @brief Pack a obs_wind message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param wind - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_wind_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const float *wind) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_WIND_LEN]; - - _mav_put_float_array(buf, 0, wind, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_WIND_LEN); -#else - mavlink_obs_wind_t packet; - - mav_array_memcpy(packet.wind, wind, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_WIND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif -} - -/** - * @brief Pack a obs_wind message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param wind - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_wind_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const float *wind) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_WIND_LEN]; - - _mav_put_float_array(buf, 0, wind, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_WIND_LEN); -#else - mavlink_obs_wind_t packet; - - mav_array_memcpy(packet.wind, wind, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_WIND; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif -} - -/** - * @brief Encode a obs_wind struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param obs_wind C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_wind_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind) -{ - return mavlink_msg_obs_wind_pack(system_id, component_id, msg, obs_wind->wind); -} - -/** - * @brief Encode a obs_wind struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param obs_wind C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind) -{ - return mavlink_msg_obs_wind_pack_chan(system_id, component_id, chan, msg, obs_wind->wind); -} - -/** - * @brief Send a obs_wind message - * @param chan MAVLink channel to send the message - * - * @param wind - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_wind_send(mavlink_channel_t chan, const float *wind) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OBS_WIND_LEN]; - - _mav_put_float_array(buf, 0, wind, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif -#else - mavlink_obs_wind_t packet; - - mav_array_memcpy(packet.wind, wind, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)&packet, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)&packet, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_OBS_WIND_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_obs_wind_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const float *wind) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - - _mav_put_float_array(buf, 0, wind, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif -#else - mavlink_obs_wind_t *packet = (mavlink_obs_wind_t *)msgbuf; - - mav_array_memcpy(packet->wind, wind, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)packet, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)packet, MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE OBS_WIND UNPACKING - - -/** - * @brief Get field wind from obs_wind message - * - * @return - - - */ -static inline uint16_t mavlink_msg_obs_wind_get_wind(const mavlink_message_t* msg, float *wind) -{ - return _MAV_RETURN_float_array(msg, wind, 3, 0); -} - -/** - * @brief Decode a obs_wind message into a struct - * - * @param msg The message to decode - * @param obs_wind C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_wind_decode(const mavlink_message_t* msg, mavlink_obs_wind_t* obs_wind) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_obs_wind_get_wind(msg, obs_wind->wind); -#else - memcpy(obs_wind, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_WIND_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h deleted file mode 100644 index 70524f464..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h +++ /dev/null @@ -1,279 +0,0 @@ -// MESSAGE PM_ELEC PACKING - -#define MAVLINK_MSG_ID_PM_ELEC 188 - -typedef struct __mavlink_pm_elec_t -{ - float PwCons; ///< - - - float BatStat; ///< - - - float PwGen[3]; ///< - - -} mavlink_pm_elec_t; - -#define MAVLINK_MSG_ID_PM_ELEC_LEN 20 -#define MAVLINK_MSG_ID_188_LEN 20 - -#define MAVLINK_MSG_ID_PM_ELEC_CRC 170 -#define MAVLINK_MSG_ID_188_CRC 170 - -#define MAVLINK_MSG_PM_ELEC_FIELD_PWGEN_LEN 3 - -#define MAVLINK_MESSAGE_INFO_PM_ELEC { \ - "PM_ELEC", \ - 3, \ - { { "PwCons", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_pm_elec_t, PwCons) }, \ - { "BatStat", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_pm_elec_t, BatStat) }, \ - { "PwGen", NULL, MAVLINK_TYPE_FLOAT, 3, 8, offsetof(mavlink_pm_elec_t, PwGen) }, \ - } \ -} - - -/** - * @brief Pack a pm_elec message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param PwCons - - - * @param BatStat - - - * @param PwGen - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pm_elec_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float PwCons, float BatStat, const float *PwGen) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PM_ELEC_LEN]; - _mav_put_float(buf, 0, PwCons); - _mav_put_float(buf, 4, BatStat); - _mav_put_float_array(buf, 8, PwGen, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PM_ELEC_LEN); -#else - mavlink_pm_elec_t packet; - packet.PwCons = PwCons; - packet.BatStat = BatStat; - mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PM_ELEC; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif -} - -/** - * @brief Pack a pm_elec message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param PwCons - - - * @param BatStat - - - * @param PwGen - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pm_elec_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float PwCons,float BatStat,const float *PwGen) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PM_ELEC_LEN]; - _mav_put_float(buf, 0, PwCons); - _mav_put_float(buf, 4, BatStat); - _mav_put_float_array(buf, 8, PwGen, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PM_ELEC_LEN); -#else - mavlink_pm_elec_t packet; - packet.PwCons = PwCons; - packet.BatStat = BatStat; - mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PM_ELEC; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif -} - -/** - * @brief Encode a pm_elec struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param pm_elec C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_pm_elec_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_pm_elec_t* pm_elec) -{ - return mavlink_msg_pm_elec_pack(system_id, component_id, msg, pm_elec->PwCons, pm_elec->BatStat, pm_elec->PwGen); -} - -/** - * @brief Encode a pm_elec struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param pm_elec C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_pm_elec_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pm_elec_t* pm_elec) -{ - return mavlink_msg_pm_elec_pack_chan(system_id, component_id, chan, msg, pm_elec->PwCons, pm_elec->BatStat, pm_elec->PwGen); -} - -/** - * @brief Send a pm_elec message - * @param chan MAVLink channel to send the message - * - * @param PwCons - - - * @param BatStat - - - * @param PwGen - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_pm_elec_send(mavlink_channel_t chan, float PwCons, float BatStat, const float *PwGen) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PM_ELEC_LEN]; - _mav_put_float(buf, 0, PwCons); - _mav_put_float(buf, 4, BatStat); - _mav_put_float_array(buf, 8, PwGen, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif -#else - mavlink_pm_elec_t packet; - packet.PwCons = PwCons; - packet.BatStat = BatStat; - mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)&packet, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)&packet, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_PM_ELEC_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_pm_elec_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float PwCons, float BatStat, const float *PwGen) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, PwCons); - _mav_put_float(buf, 4, BatStat); - _mav_put_float_array(buf, 8, PwGen, 3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif -#else - mavlink_pm_elec_t *packet = (mavlink_pm_elec_t *)msgbuf; - packet->PwCons = PwCons; - packet->BatStat = BatStat; - mav_array_memcpy(packet->PwGen, PwGen, sizeof(float)*3); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)packet, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)packet, MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE PM_ELEC UNPACKING - - -/** - * @brief Get field PwCons from pm_elec message - * - * @return - - - */ -static inline float mavlink_msg_pm_elec_get_PwCons(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field BatStat from pm_elec message - * - * @return - - - */ -static inline float mavlink_msg_pm_elec_get_BatStat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field PwGen from pm_elec message - * - * @return - - - */ -static inline uint16_t mavlink_msg_pm_elec_get_PwGen(const mavlink_message_t* msg, float *PwGen) -{ - return _MAV_RETURN_float_array(msg, PwGen, 3, 8); -} - -/** - * @brief Decode a pm_elec message into a struct - * - * @param msg The message to decode - * @param pm_elec C-struct to decode the message contents into - */ -static inline void mavlink_msg_pm_elec_decode(const mavlink_message_t* msg, mavlink_pm_elec_t* pm_elec) -{ -#if MAVLINK_NEED_BYTE_SWAP - pm_elec->PwCons = mavlink_msg_pm_elec_get_PwCons(msg); - pm_elec->BatStat = mavlink_msg_pm_elec_get_BatStat(msg); - mavlink_msg_pm_elec_get_PwGen(msg, pm_elec->PwGen); -#else - memcpy(pm_elec, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PM_ELEC_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h deleted file mode 100644 index 72f8a8193..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h +++ /dev/null @@ -1,321 +0,0 @@ -// MESSAGE SYS_Stat PACKING - -#define MAVLINK_MSG_ID_SYS_Stat 190 - -typedef struct __mavlink_sys_stat_t -{ - uint8_t gps; ///< - - - uint8_t act; ///< - - - uint8_t mod; ///< - - - uint8_t commRssi; ///< - - -} mavlink_sys_stat_t; - -#define MAVLINK_MSG_ID_SYS_Stat_LEN 4 -#define MAVLINK_MSG_ID_190_LEN 4 - -#define MAVLINK_MSG_ID_SYS_Stat_CRC 157 -#define MAVLINK_MSG_ID_190_CRC 157 - - - -#define MAVLINK_MESSAGE_INFO_SYS_Stat { \ - "SYS_Stat", \ - 4, \ - { { "gps", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_sys_stat_t, gps) }, \ - { "act", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_sys_stat_t, act) }, \ - { "mod", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_sys_stat_t, mod) }, \ - { "commRssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_sys_stat_t, commRssi) }, \ - } \ -} - - -/** - * @brief Pack a sys_stat message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param gps - - - * @param act - - - * @param mod - - - * @param commRssi - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sys_stat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t gps, uint8_t act, uint8_t mod, uint8_t commRssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SYS_Stat_LEN]; - _mav_put_uint8_t(buf, 0, gps); - _mav_put_uint8_t(buf, 1, act); - _mav_put_uint8_t(buf, 2, mod); - _mav_put_uint8_t(buf, 3, commRssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_Stat_LEN); -#else - mavlink_sys_stat_t packet; - packet.gps = gps; - packet.act = act; - packet.mod = mod; - packet.commRssi = commRssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYS_Stat; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); -#else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif -} - -/** - * @brief Pack a sys_stat message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param gps - - - * @param act - - - * @param mod - - - * @param commRssi - - - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sys_stat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t gps,uint8_t act,uint8_t mod,uint8_t commRssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SYS_Stat_LEN]; - _mav_put_uint8_t(buf, 0, gps); - _mav_put_uint8_t(buf, 1, act); - _mav_put_uint8_t(buf, 2, mod); - _mav_put_uint8_t(buf, 3, commRssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_Stat_LEN); -#else - mavlink_sys_stat_t packet; - packet.gps = gps; - packet.act = act; - packet.mod = mod; - packet.commRssi = commRssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYS_Stat; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif -} - -/** - * @brief Encode a sys_stat struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param sys_stat C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sys_stat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_stat_t* sys_stat) -{ - return mavlink_msg_sys_stat_pack(system_id, component_id, msg, sys_stat->gps, sys_stat->act, sys_stat->mod, sys_stat->commRssi); -} - -/** - * @brief Encode a sys_stat struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param sys_stat C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sys_stat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_stat_t* sys_stat) -{ - return mavlink_msg_sys_stat_pack_chan(system_id, component_id, chan, msg, sys_stat->gps, sys_stat->act, sys_stat->mod, sys_stat->commRssi); -} - -/** - * @brief Send a sys_stat message - * @param chan MAVLink channel to send the message - * - * @param gps - - - * @param act - - - * @param mod - - - * @param commRssi - - - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_sys_stat_send(mavlink_channel_t chan, uint8_t gps, uint8_t act, uint8_t mod, uint8_t commRssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SYS_Stat_LEN]; - _mav_put_uint8_t(buf, 0, gps); - _mav_put_uint8_t(buf, 1, act); - _mav_put_uint8_t(buf, 2, mod); - _mav_put_uint8_t(buf, 3, commRssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif -#else - mavlink_sys_stat_t packet; - packet.gps = gps; - packet.act = act; - packet.mod = mod; - packet.commRssi = commRssi; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)&packet, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)&packet, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif -#endif -} - -#if MAVLINK_MSG_ID_SYS_Stat_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_sys_stat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t gps, uint8_t act, uint8_t mod, uint8_t commRssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, gps); - _mav_put_uint8_t(buf, 1, act); - _mav_put_uint8_t(buf, 2, mod); - _mav_put_uint8_t(buf, 3, commRssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif -#else - mavlink_sys_stat_t *packet = (mavlink_sys_stat_t *)msgbuf; - packet->gps = gps; - packet->act = act; - packet->mod = mod; - packet->commRssi = commRssi; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)packet, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)packet, MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif -#endif -} -#endif - -#endif - -// MESSAGE SYS_Stat UNPACKING - - -/** - * @brief Get field gps from sys_stat message - * - * @return - - - */ -static inline uint8_t mavlink_msg_sys_stat_get_gps(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field act from sys_stat message - * - * @return - - - */ -static inline uint8_t mavlink_msg_sys_stat_get_act(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field mod from sys_stat message - * - * @return - - - */ -static inline uint8_t mavlink_msg_sys_stat_get_mod(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field commRssi from sys_stat message - * - * @return - - - */ -static inline uint8_t mavlink_msg_sys_stat_get_commRssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Decode a sys_stat message into a struct - * - * @param msg The message to decode - * @param sys_stat C-struct to decode the message contents into - */ -static inline void mavlink_msg_sys_stat_decode(const mavlink_message_t* msg, mavlink_sys_stat_t* sys_stat) -{ -#if MAVLINK_NEED_BYTE_SWAP - sys_stat->gps = mavlink_msg_sys_stat_get_gps(msg); - sys_stat->act = mavlink_msg_sys_stat_get_act(msg); - sys_stat->mod = mavlink_msg_sys_stat_get_mod(msg); - sys_stat->commRssi = mavlink_msg_sys_stat_get_commRssi(msg); -#else - memcpy(sys_stat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SYS_Stat_LEN); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h b/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h deleted file mode 100644 index 2eab86354..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h +++ /dev/null @@ -1,77 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from sensesoar.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef SENSESOAR_H -#define SENSESOAR_H - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_POSITION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_ATTITUDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_WIND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_BIAS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_QFF, MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP, MAVLINK_MESSAGE_INFO_FILT_ROT_VEL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LLC_OUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PM_ELEC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_Stat, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_SENSESOAR - -// ENUM DEFINITIONS - - -/** @brief Different flight modes */ -#ifndef HAVE_ENUM_SENSESOAR_MODE -#define HAVE_ENUM_SENSESOAR_MODE -enum SENSESOAR_MODE -{ - SENSESOAR_MODE_GLIDING=1, /* Gliding mode with motors off | */ - SENSESOAR_MODE_AUTONOMOUS=2, /* Autonomous flight | */ - SENSESOAR_MODE_MANUAL=3, /* RC controlled | */ - SENSESOAR_MODE_ENUM_END=4, /* | */ -}; -#endif - -#include "../common/common.h" - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_obs_position.h" -#include "./mavlink_msg_obs_velocity.h" -#include "./mavlink_msg_obs_attitude.h" -#include "./mavlink_msg_obs_wind.h" -#include "./mavlink_msg_obs_air_velocity.h" -#include "./mavlink_msg_obs_bias.h" -#include "./mavlink_msg_obs_qff.h" -#include "./mavlink_msg_obs_air_temp.h" -#include "./mavlink_msg_filt_rot_vel.h" -#include "./mavlink_msg_llc_out.h" -#include "./mavlink_msg_pm_elec.h" -#include "./mavlink_msg_sys_stat.h" -#include "./mavlink_msg_cmd_airspeed_chng.h" -#include "./mavlink_msg_cmd_airspeed_ack.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // SENSESOAR_H diff --git a/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h b/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h deleted file mode 100644 index 67ffca799..000000000 --- a/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h +++ /dev/null @@ -1,676 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from sensesoar.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef SENSESOAR_TESTSUITE_H -#define SENSESOAR_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_sensesoar(uint8_t, uint8_t, mavlink_message_t *last_msg); - -static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_test_common(system_id, component_id, last_msg); - mavlink_test_sensesoar(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_obs_position(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_obs_position_t packet_in = { - 963497464, - }963497672, - }963497880, - }; - mavlink_obs_position_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.lon = packet_in.lon; - packet1.lat = packet_in.lat; - packet1.alt = packet_in.alt; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_obs_position_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_obs_position_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_obs_position_pack(system_id, component_id, &msg , packet1.lon , packet1.lat , packet1.alt ); - mavlink_msg_obs_position_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_obs_position_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.lon , packet1.lat , packet1.alt ); - mavlink_msg_obs_position_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; i Date: Wed, 2 Jul 2014 14:18:55 +0200 Subject: mavlink as git submodule Mavlink is now a git submodule because auto generated header files are available at https://github.com/mavlink/c_library --- .gitmodules | 3 +++ mavlink/include/mavlink/v1.0 | 1 + 2 files changed, 4 insertions(+) create mode 100644 .gitmodules create mode 160000 mavlink/include/mavlink/v1.0 diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..946a1fdcf --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "mavlink/include/mavlink/v1.0"] + path = mavlink/include/mavlink/v1.0 + url = git@github.com:mavlink/c_library.git diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 new file mode 160000 index 000000000..45a71d656 --- /dev/null +++ b/mavlink/include/mavlink/v1.0 @@ -0,0 +1 @@ +Subproject commit 45a71d6564bc5c47ed97d620089e17ca48bab73f -- cgit v1.2.3 From 8f7fc27c5b6fe2dfe003ae1b421b83c23500415f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 2 Jul 2014 15:40:28 +0200 Subject: sync with mavlink externalsetpoint branch https://github.com/mavlink/mavlink/commit/0d55cc7e886f6d3c824f8436a39b9f9b36d0458f --- .../mavlink/v1.0/ardupilotmega/ardupilotmega.h | 10 +- .../include/mavlink/v1.0/ardupilotmega/version.h | 2 +- mavlink/include/mavlink/v1.0/autoquad/autoquad.h | 6 +- mavlink/include/mavlink/v1.0/autoquad/version.h | 2 +- mavlink/include/mavlink/v1.0/common/common.h | 8 +- ...ink_msg_global_position_setpoint_external_int.h | 152 +++---- .../mavlink/v1.0/common/mavlink_msg_gps2_raw.h | 10 +- .../mavlink/v1.0/common/mavlink_msg_gps2_rtk.h | 497 +++++++++++++++++++++ .../mavlink/v1.0/common/mavlink_msg_gps_raw_int.h | 10 +- .../mavlink/v1.0/common/mavlink_msg_gps_rtk.h | 497 +++++++++++++++++++++ ...link_msg_local_ned_position_setpoint_external.h | 152 +++---- mavlink/include/mavlink/v1.0/common/testsuite.h | 164 ++++++- mavlink/include/mavlink/v1.0/common/version.h | 2 +- .../include/mavlink/v1.0/matrixpilot/matrixpilot.h | 6 +- mavlink/include/mavlink/v1.0/matrixpilot/version.h | 2 +- mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h | 6 +- mavlink/include/mavlink/v1.0/pixhawk/version.h | 2 +- mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h | 6 +- mavlink/include/mavlink/v1.0/sensesoar/version.h | 2 +- 19 files changed, 1335 insertions(+), 201 deletions(-) create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_rtk.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_rtk.h diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index 7e9173247..d3213e7c9 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -16,15 +16,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 39, 45, 44, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 20, 24, 29, 41, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 39, 45, 44, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 35, 35, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 20, 24, 29, 41, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 212, 121, 172, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 206, 151, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 212, 211, 198, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 206, 151, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_ATTITUDE_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS, MAVLINK_MESSAGE_INFO_AHRS2, MAVLINK_MESSAGE_INFO_CAMERA_EVENT, MAVLINK_MESSAGE_INFO_CAMERA_FEEDBACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_ATTITUDE_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS, MAVLINK_MESSAGE_INFO_AHRS2, MAVLINK_MESSAGE_INFO_CAMERA_EVENT, MAVLINK_MESSAGE_INFO_CAMERA_FEEDBACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -174,7 +174,9 @@ typedef enum CAMERA_FEEDBACK_FLAGS { VIDEO=1, /* Shooting video, not stills | */ BADEXPOSURE=2, /* Unable to achieve requested exposure (e.g. shutter speed too low) | */ - CAMERA_FEEDBACK_FLAGS_ENUM_END=3, /* | */ + CLOSEDLOOP=3, /* Closed loop feedback from camera, we know for sure it has successfully taken a picture | */ + OPENLOOP=4, /* Open loop camera, an image trigger has been requested but we can't know for sure it has successfully taken a picture | */ + CAMERA_FEEDBACK_FLAGS_ENUM_END=5, /* | */ } CAMERA_FEEDBACK_FLAGS; #endif diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h index 192d1cb01..32077820d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Mon Jun 23 10:13:03 2014" +#define MAVLINK_BUILD_DATE "Wed Jul 2 15:36:07 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h index 464994f86..0428100ca 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h +++ b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h @@ -16,15 +16,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 39, 45, 44, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 39, 45, 44, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 35, 35, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 212, 121, 172, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 241, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 212, 211, 198, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 241, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_ATTITUDE_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_AQ_TELEMETRY_F, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_ATTITUDE_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_AQ_TELEMETRY_F, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" diff --git a/mavlink/include/mavlink/v1.0/autoquad/version.h b/mavlink/include/mavlink/v1.0/autoquad/version.h index ce27e3e53..3a6222d73 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/version.h +++ b/mavlink/include/mavlink/v1.0/autoquad/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Mon Jun 23 10:13:13 2014" +#define MAVLINK_BUILD_DATE "Wed Jul 2 15:36:18 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h index 92186cc8a..f42c066a6 100644 --- a/mavlink/include/mavlink/v1.0/common/common.h +++ b/mavlink/include/mavlink/v1.0/common/common.h @@ -16,15 +16,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 39, 45, 44, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 39, 45, 44, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 35, 35, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 212, 121, 172, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 212, 211, 198, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_ATTITUDE_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_ATTITUDE_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_LOCAL_NED_POSITION_SETPOINT_EXTERNAL, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -659,6 +659,8 @@ typedef enum MAV_DISTANCE_SENSOR #include "./mavlink_msg_gps2_raw.h" #include "./mavlink_msg_power_status.h" #include "./mavlink_msg_serial_control.h" +#include "./mavlink_msg_gps_rtk.h" +#include "./mavlink_msg_gps2_rtk.h" #include "./mavlink_msg_data_transmission_handshake.h" #include "./mavlink_msg_encapsulated_data.h" #include "./mavlink_msg_distance_sensor.h" diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_external_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_external_int.h index 46ab556d8..a383def66 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_external_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_external_int.h @@ -11,10 +11,10 @@ typedef struct __mavlink_global_position_setpoint_external_int_t float vx; ///< X velocity in NED frame in meter / s float vy; ///< Y velocity in NED frame in meter / s float vz; ///< Z velocity in NED frame in meter / s - float ax; ///< X acceleration in NED frame in meter / s^2 - float ay; ///< Y acceleration in NED frame in meter / s^2 - float az; ///< Z acceleration in NED frame in meter / s^2 - uint16_t ignore_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: lat, bit 2: lon, bit 3: alt, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + float afx; ///< X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afy; ///< Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afz; ///< Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + uint16_t type_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID } mavlink_global_position_setpoint_external_int_t; @@ -22,8 +22,8 @@ typedef struct __mavlink_global_position_setpoint_external_int_t #define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN 44 #define MAVLINK_MSG_ID_84_LEN 44 -#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_CRC 172 -#define MAVLINK_MSG_ID_84_CRC 172 +#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_CRC 198 +#define MAVLINK_MSG_ID_84_CRC 198 @@ -37,10 +37,10 @@ typedef struct __mavlink_global_position_setpoint_external_int_t { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_global_position_setpoint_external_int_t, vx) }, \ { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_position_setpoint_external_int_t, vy) }, \ { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_position_setpoint_external_int_t, vz) }, \ - { "ax", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_position_setpoint_external_int_t, ax) }, \ - { "ay", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_global_position_setpoint_external_int_t, ay) }, \ - { "az", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_global_position_setpoint_external_int_t, az) }, \ - { "ignore_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_global_position_setpoint_external_int_t, ignore_mask) }, \ + { "afx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_position_setpoint_external_int_t, afx) }, \ + { "afy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_global_position_setpoint_external_int_t, afy) }, \ + { "afz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_global_position_setpoint_external_int_t, afz) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_global_position_setpoint_external_int_t, type_mask) }, \ { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_global_position_setpoint_external_int_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_global_position_setpoint_external_int_t, target_component) }, \ } \ @@ -56,20 +56,20 @@ typedef struct __mavlink_global_position_setpoint_external_int_t * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. * @param target_system System ID * @param target_component Component ID - * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: lat, bit 2: lon, bit 3: alt, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint * @param lat_int X Position in WGS84 frame in 1e7 * meters * @param lon_int Y Position in WGS84 frame in 1e7 * meters * @param alt Altitude in WGS84, not AMSL * @param vx X velocity in NED frame in meter / s * @param vy Y velocity in NED frame in meter / s * @param vz Z velocity in NED frame in meter / s - * @param ax X acceleration in NED frame in meter / s^2 - * @param ay Y acceleration in NED frame in meter / s^2 - * @param az Z acceleration in NED frame in meter / s^2 + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_position_setpoint_external_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint16_t ignore_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float ax, float ay, float az) + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN]; @@ -80,10 +80,10 @@ static inline uint16_t mavlink_msg_global_position_setpoint_external_int_pack(ui _mav_put_float(buf, 16, vx); _mav_put_float(buf, 20, vy); _mav_put_float(buf, 24, vz); - _mav_put_float(buf, 28, ax); - _mav_put_float(buf, 32, ay); - _mav_put_float(buf, 36, az); - _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_uint16_t(buf, 40, type_mask); _mav_put_uint8_t(buf, 42, target_system); _mav_put_uint8_t(buf, 43, target_component); @@ -97,10 +97,10 @@ static inline uint16_t mavlink_msg_global_position_setpoint_external_int_pack(ui packet.vx = vx; packet.vy = vy; packet.vz = vz; - packet.ax = ax; - packet.ay = ay; - packet.az = az; - packet.ignore_mask = ignore_mask; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.type_mask = type_mask; packet.target_system = target_system; packet.target_component = target_component; @@ -124,21 +124,21 @@ static inline uint16_t mavlink_msg_global_position_setpoint_external_int_pack(ui * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. * @param target_system System ID * @param target_component Component ID - * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: lat, bit 2: lon, bit 3: alt, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint * @param lat_int X Position in WGS84 frame in 1e7 * meters * @param lon_int Y Position in WGS84 frame in 1e7 * meters * @param alt Altitude in WGS84, not AMSL * @param vx X velocity in NED frame in meter / s * @param vy Y velocity in NED frame in meter / s * @param vz Z velocity in NED frame in meter / s - * @param ax X acceleration in NED frame in meter / s^2 - * @param ay Y acceleration in NED frame in meter / s^2 - * @param az Z acceleration in NED frame in meter / s^2 + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_position_setpoint_external_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint16_t ignore_mask,int32_t lat_int,int32_t lon_int,float alt,float vx,float vy,float vz,float ax,float ay,float az) + uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint16_t type_mask,int32_t lat_int,int32_t lon_int,float alt,float vx,float vy,float vz,float afx,float afy,float afz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN]; @@ -149,10 +149,10 @@ static inline uint16_t mavlink_msg_global_position_setpoint_external_int_pack_ch _mav_put_float(buf, 16, vx); _mav_put_float(buf, 20, vy); _mav_put_float(buf, 24, vz); - _mav_put_float(buf, 28, ax); - _mav_put_float(buf, 32, ay); - _mav_put_float(buf, 36, az); - _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_uint16_t(buf, 40, type_mask); _mav_put_uint8_t(buf, 42, target_system); _mav_put_uint8_t(buf, 43, target_component); @@ -166,10 +166,10 @@ static inline uint16_t mavlink_msg_global_position_setpoint_external_int_pack_ch packet.vx = vx; packet.vy = vy; packet.vz = vz; - packet.ax = ax; - packet.ay = ay; - packet.az = az; - packet.ignore_mask = ignore_mask; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.type_mask = type_mask; packet.target_system = target_system; packet.target_component = target_component; @@ -194,7 +194,7 @@ static inline uint16_t mavlink_msg_global_position_setpoint_external_int_pack_ch */ static inline uint16_t mavlink_msg_global_position_setpoint_external_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_setpoint_external_int_t* global_position_setpoint_external_int) { - return mavlink_msg_global_position_setpoint_external_int_pack(system_id, component_id, msg, global_position_setpoint_external_int->time_boot_ms, global_position_setpoint_external_int->target_system, global_position_setpoint_external_int->target_component, global_position_setpoint_external_int->ignore_mask, global_position_setpoint_external_int->lat_int, global_position_setpoint_external_int->lon_int, global_position_setpoint_external_int->alt, global_position_setpoint_external_int->vx, global_position_setpoint_external_int->vy, global_position_setpoint_external_int->vz, global_position_setpoint_external_int->ax, global_position_setpoint_external_int->ay, global_position_setpoint_external_int->az); + return mavlink_msg_global_position_setpoint_external_int_pack(system_id, component_id, msg, global_position_setpoint_external_int->time_boot_ms, global_position_setpoint_external_int->target_system, global_position_setpoint_external_int->target_component, global_position_setpoint_external_int->type_mask, global_position_setpoint_external_int->lat_int, global_position_setpoint_external_int->lon_int, global_position_setpoint_external_int->alt, global_position_setpoint_external_int->vx, global_position_setpoint_external_int->vy, global_position_setpoint_external_int->vz, global_position_setpoint_external_int->afx, global_position_setpoint_external_int->afy, global_position_setpoint_external_int->afz); } /** @@ -208,7 +208,7 @@ static inline uint16_t mavlink_msg_global_position_setpoint_external_int_encode( */ static inline uint16_t mavlink_msg_global_position_setpoint_external_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_setpoint_external_int_t* global_position_setpoint_external_int) { - return mavlink_msg_global_position_setpoint_external_int_pack_chan(system_id, component_id, chan, msg, global_position_setpoint_external_int->time_boot_ms, global_position_setpoint_external_int->target_system, global_position_setpoint_external_int->target_component, global_position_setpoint_external_int->ignore_mask, global_position_setpoint_external_int->lat_int, global_position_setpoint_external_int->lon_int, global_position_setpoint_external_int->alt, global_position_setpoint_external_int->vx, global_position_setpoint_external_int->vy, global_position_setpoint_external_int->vz, global_position_setpoint_external_int->ax, global_position_setpoint_external_int->ay, global_position_setpoint_external_int->az); + return mavlink_msg_global_position_setpoint_external_int_pack_chan(system_id, component_id, chan, msg, global_position_setpoint_external_int->time_boot_ms, global_position_setpoint_external_int->target_system, global_position_setpoint_external_int->target_component, global_position_setpoint_external_int->type_mask, global_position_setpoint_external_int->lat_int, global_position_setpoint_external_int->lon_int, global_position_setpoint_external_int->alt, global_position_setpoint_external_int->vx, global_position_setpoint_external_int->vy, global_position_setpoint_external_int->vz, global_position_setpoint_external_int->afx, global_position_setpoint_external_int->afy, global_position_setpoint_external_int->afz); } /** @@ -218,20 +218,20 @@ static inline uint16_t mavlink_msg_global_position_setpoint_external_int_encode_ * @param time_boot_ms Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. * @param target_system System ID * @param target_component Component ID - * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: lat, bit 2: lon, bit 3: alt, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint * @param lat_int X Position in WGS84 frame in 1e7 * meters * @param lon_int Y Position in WGS84 frame in 1e7 * meters * @param alt Altitude in WGS84, not AMSL * @param vx X velocity in NED frame in meter / s * @param vy Y velocity in NED frame in meter / s * @param vz Z velocity in NED frame in meter / s - * @param ax X acceleration in NED frame in meter / s^2 - * @param ay Y acceleration in NED frame in meter / s^2 - * @param az Z acceleration in NED frame in meter / s^2 + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_global_position_setpoint_external_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint16_t ignore_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float ax, float ay, float az) +static inline void mavlink_msg_global_position_setpoint_external_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_EXTERNAL_INT_LEN]; @@ -242,10 +242,10 @@ static inline void mavlink_msg_global_position_setpoint_external_int_send(mavlin _mav_put_float(buf, 16, vx); _mav_put_float(buf, 20, vy); _mav_put_float(buf, 24, vz); - _mav_put_float(buf, 28, ax); - _mav_put_float(buf, 32, ay); - _mav_put_float(buf, 36, az); - _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_uint16_t(buf, 40, type_mask); _mav_put_uint8_t(buf, 42, target_system); _mav_put_uint8_t(buf, 43, target_component); @@ -263,10 +263,10 @@ static inline void mavlink_msg_global_position_setpoint_external_int_send(mavlin packet.vx = vx; packet.vy = vy; packet.vz = vz; - packet.ax = ax; - packet.ay = ay; - packet.az = az; - packet.ignore_mask = ignore_mask; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.type_mask = type_mask; packet.target_system = target_system; packet.target_component = target_component; @@ -286,7 +286,7 @@ static inline void mavlink_msg_global_position_setpoint_external_int_send(mavlin is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_global_position_setpoint_external_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint16_t ignore_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float ax, float ay, float az) +static inline void mavlink_msg_global_position_setpoint_external_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -297,10 +297,10 @@ static inline void mavlink_msg_global_position_setpoint_external_int_send_buf(ma _mav_put_float(buf, 16, vx); _mav_put_float(buf, 20, vy); _mav_put_float(buf, 24, vz); - _mav_put_float(buf, 28, ax); - _mav_put_float(buf, 32, ay); - _mav_put_float(buf, 36, az); - _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_uint16_t(buf, 40, type_mask); _mav_put_uint8_t(buf, 42, target_system); _mav_put_uint8_t(buf, 43, target_component); @@ -318,10 +318,10 @@ static inline void mavlink_msg_global_position_setpoint_external_int_send_buf(ma packet->vx = vx; packet->vy = vy; packet->vz = vz; - packet->ax = ax; - packet->ay = ay; - packet->az = az; - packet->ignore_mask = ignore_mask; + packet->afx = afx; + packet->afy = afy; + packet->afz = afz; + packet->type_mask = type_mask; packet->target_system = target_system; packet->target_component = target_component; @@ -370,11 +370,11 @@ static inline uint8_t mavlink_msg_global_position_setpoint_external_int_get_targ } /** - * @brief Get field ignore_mask from global_position_setpoint_external_int message + * @brief Get field type_mask from global_position_setpoint_external_int message * - * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: lat, bit 2: lon, bit 3: alt, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint */ -static inline uint16_t mavlink_msg_global_position_setpoint_external_int_get_ignore_mask(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_global_position_setpoint_external_int_get_type_mask(const mavlink_message_t* msg) { return _MAV_RETURN_uint16_t(msg, 40); } @@ -440,31 +440,31 @@ static inline float mavlink_msg_global_position_setpoint_external_int_get_vz(con } /** - * @brief Get field ax from global_position_setpoint_external_int message + * @brief Get field afx from global_position_setpoint_external_int message * - * @return X acceleration in NED frame in meter / s^2 + * @return X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N */ -static inline float mavlink_msg_global_position_setpoint_external_int_get_ax(const mavlink_message_t* msg) +static inline float mavlink_msg_global_position_setpoint_external_int_get_afx(const mavlink_message_t* msg) { return _MAV_RETURN_float(msg, 28); } /** - * @brief Get field ay from global_position_setpoint_external_int message + * @brief Get field afy from global_position_setpoint_external_int message * - * @return Y acceleration in NED frame in meter / s^2 + * @return Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N */ -static inline float mavlink_msg_global_position_setpoint_external_int_get_ay(const mavlink_message_t* msg) +static inline float mavlink_msg_global_position_setpoint_external_int_get_afy(const mavlink_message_t* msg) { return _MAV_RETURN_float(msg, 32); } /** - * @brief Get field az from global_position_setpoint_external_int message + * @brief Get field afz from global_position_setpoint_external_int message * - * @return Z acceleration in NED frame in meter / s^2 + * @return Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N */ -static inline float mavlink_msg_global_position_setpoint_external_int_get_az(const mavlink_message_t* msg) +static inline float mavlink_msg_global_position_setpoint_external_int_get_afz(const mavlink_message_t* msg) { return _MAV_RETURN_float(msg, 36); } @@ -485,10 +485,10 @@ static inline void mavlink_msg_global_position_setpoint_external_int_decode(cons global_position_setpoint_external_int->vx = mavlink_msg_global_position_setpoint_external_int_get_vx(msg); global_position_setpoint_external_int->vy = mavlink_msg_global_position_setpoint_external_int_get_vy(msg); global_position_setpoint_external_int->vz = mavlink_msg_global_position_setpoint_external_int_get_vz(msg); - global_position_setpoint_external_int->ax = mavlink_msg_global_position_setpoint_external_int_get_ax(msg); - global_position_setpoint_external_int->ay = mavlink_msg_global_position_setpoint_external_int_get_ay(msg); - global_position_setpoint_external_int->az = mavlink_msg_global_position_setpoint_external_int_get_az(msg); - global_position_setpoint_external_int->ignore_mask = mavlink_msg_global_position_setpoint_external_int_get_ignore_mask(msg); + global_position_setpoint_external_int->afx = mavlink_msg_global_position_setpoint_external_int_get_afx(msg); + global_position_setpoint_external_int->afy = mavlink_msg_global_position_setpoint_external_int_get_afy(msg); + global_position_setpoint_external_int->afz = mavlink_msg_global_position_setpoint_external_int_get_afz(msg); + global_position_setpoint_external_int->type_mask = mavlink_msg_global_position_setpoint_external_int_get_type_mask(msg); global_position_setpoint_external_int->target_system = mavlink_msg_global_position_setpoint_external_int_get_target_system(msg); global_position_setpoint_external_int->target_component = mavlink_msg_global_position_setpoint_external_int_get_target_component(msg); #else diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h index b184e7c9c..84f15bf63 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h @@ -13,7 +13,7 @@ typedef struct __mavlink_gps2_raw_t uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 uint8_t dgps_numch; ///< Number of DGPS satellites } mavlink_gps2_raw_t; @@ -52,7 +52,7 @@ typedef struct __mavlink_gps2_raw_t * @param msg The MAVLink message to compress the data into * * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t comp * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) @@ -208,7 +208,7 @@ static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8 * @param chan MAVLink channel to send the message * * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) @@ -339,7 +339,7 @@ static inline uint64_t mavlink_msg_gps2_raw_get_time_usec(const mavlink_message_ /** * @brief Get field fix_type from gps2_raw message * - * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @return 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS fix, 5: RTK Fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ static inline uint8_t mavlink_msg_gps2_raw_get_fix_type(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_rtk.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_rtk.h new file mode 100644 index 000000000..4c2d93c6e --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_rtk.h @@ -0,0 +1,497 @@ +// MESSAGE GPS2_RTK PACKING + +#define MAVLINK_MSG_ID_GPS2_RTK 128 + +typedef struct __mavlink_gps2_rtk_t +{ + uint32_t time_last_baseline_ms; ///< Time since boot of last baseline message received in ms. + uint32_t tow; ///< GPS Time of Week of last baseline + int32_t baseline_a_mm; ///< Current baseline in ECEF x or NED north component in mm. + int32_t baseline_b_mm; ///< Current baseline in ECEF y or NED east component in mm. + int32_t baseline_c_mm; ///< Current baseline in ECEF z or NED down component in mm. + uint32_t accuracy; ///< Current estimate of baseline accuracy. + int32_t iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses. + uint16_t wn; ///< GPS Week Number of last baseline + uint8_t rtk_receiver_id; ///< Identification of connected RTK receiver. + uint8_t rtk_health; ///< GPS-specific health report for RTK data. + uint8_t rtk_rate; ///< Rate of baseline messages being received by GPS, in HZ + uint8_t nsats; ///< Current number of sats used for RTK calculation. + uint8_t baseline_coords_type; ///< Coordinate system of baseline. 0 == ECEF, 1 == NED +} mavlink_gps2_rtk_t; + +#define MAVLINK_MSG_ID_GPS2_RTK_LEN 35 +#define MAVLINK_MSG_ID_128_LEN 35 + +#define MAVLINK_MSG_ID_GPS2_RTK_CRC 226 +#define MAVLINK_MSG_ID_128_CRC 226 + + + +#define MAVLINK_MESSAGE_INFO_GPS2_RTK { \ + "GPS2_RTK", \ + 13, \ + { { "time_last_baseline_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gps2_rtk_t, time_last_baseline_ms) }, \ + { "tow", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gps2_rtk_t, tow) }, \ + { "baseline_a_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_rtk_t, baseline_a_mm) }, \ + { "baseline_b_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps2_rtk_t, baseline_b_mm) }, \ + { "baseline_c_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps2_rtk_t, baseline_c_mm) }, \ + { "accuracy", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_rtk_t, accuracy) }, \ + { "iar_num_hypotheses", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_gps2_rtk_t, iar_num_hypotheses) }, \ + { "wn", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps2_rtk_t, wn) }, \ + { "rtk_receiver_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gps2_rtk_t, rtk_receiver_id) }, \ + { "rtk_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gps2_rtk_t, rtk_health) }, \ + { "rtk_rate", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_rtk_t, rtk_rate) }, \ + { "nsats", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_rtk_t, nsats) }, \ + { "baseline_coords_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_rtk_t, baseline_coords_type) }, \ + } \ +} + + +/** + * @brief Pack a gps2_rtk message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_rtk_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#else + mavlink_gps2_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +} + +/** + * @brief Pack a gps2_rtk message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_rtk_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_last_baseline_ms,uint8_t rtk_receiver_id,uint16_t wn,uint32_t tow,uint8_t rtk_health,uint8_t rtk_rate,uint8_t nsats,uint8_t baseline_coords_type,int32_t baseline_a_mm,int32_t baseline_b_mm,int32_t baseline_c_mm,uint32_t accuracy,int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#else + mavlink_gps2_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +} + +/** + * @brief Encode a gps2_rtk struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps2_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_rtk_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_rtk_t* gps2_rtk) +{ + return mavlink_msg_gps2_rtk_pack(system_id, component_id, msg, gps2_rtk->time_last_baseline_ms, gps2_rtk->rtk_receiver_id, gps2_rtk->wn, gps2_rtk->tow, gps2_rtk->rtk_health, gps2_rtk->rtk_rate, gps2_rtk->nsats, gps2_rtk->baseline_coords_type, gps2_rtk->baseline_a_mm, gps2_rtk->baseline_b_mm, gps2_rtk->baseline_c_mm, gps2_rtk->accuracy, gps2_rtk->iar_num_hypotheses); +} + +/** + * @brief Encode a gps2_rtk struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps2_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_rtk_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_rtk_t* gps2_rtk) +{ + return mavlink_msg_gps2_rtk_pack_chan(system_id, component_id, chan, msg, gps2_rtk->time_last_baseline_ms, gps2_rtk->rtk_receiver_id, gps2_rtk->wn, gps2_rtk->tow, gps2_rtk->rtk_health, gps2_rtk->rtk_rate, gps2_rtk->nsats, gps2_rtk->baseline_coords_type, gps2_rtk->baseline_a_mm, gps2_rtk->baseline_b_mm, gps2_rtk->baseline_c_mm, gps2_rtk->accuracy, gps2_rtk->iar_num_hypotheses); +} + +/** + * @brief Send a gps2_rtk message + * @param chan MAVLink channel to send the message + * + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps2_rtk_send(mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +#else + mavlink_gps2_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GPS2_RTK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps2_rtk_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +#else + mavlink_gps2_rtk_t *packet = (mavlink_gps2_rtk_t *)msgbuf; + packet->time_last_baseline_ms = time_last_baseline_ms; + packet->tow = tow; + packet->baseline_a_mm = baseline_a_mm; + packet->baseline_b_mm = baseline_b_mm; + packet->baseline_c_mm = baseline_c_mm; + packet->accuracy = accuracy; + packet->iar_num_hypotheses = iar_num_hypotheses; + packet->wn = wn; + packet->rtk_receiver_id = rtk_receiver_id; + packet->rtk_health = rtk_health; + packet->rtk_rate = rtk_rate; + packet->nsats = nsats; + packet->baseline_coords_type = baseline_coords_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GPS2_RTK UNPACKING + + +/** + * @brief Get field time_last_baseline_ms from gps2_rtk message + * + * @return Time since boot of last baseline message received in ms. + */ +static inline uint32_t mavlink_msg_gps2_rtk_get_time_last_baseline_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field rtk_receiver_id from gps2_rtk message + * + * @return Identification of connected RTK receiver. + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_rtk_receiver_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field wn from gps2_rtk message + * + * @return GPS Week Number of last baseline + */ +static inline uint16_t mavlink_msg_gps2_rtk_get_wn(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field tow from gps2_rtk message + * + * @return GPS Time of Week of last baseline + */ +static inline uint32_t mavlink_msg_gps2_rtk_get_tow(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field rtk_health from gps2_rtk message + * + * @return GPS-specific health report for RTK data. + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_rtk_health(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field rtk_rate from gps2_rtk message + * + * @return Rate of baseline messages being received by GPS, in HZ + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_rtk_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field nsats from gps2_rtk message + * + * @return Current number of sats used for RTK calculation. + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_nsats(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field baseline_coords_type from gps2_rtk message + * + * @return Coordinate system of baseline. 0 == ECEF, 1 == NED + */ +static inline uint8_t mavlink_msg_gps2_rtk_get_baseline_coords_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field baseline_a_mm from gps2_rtk message + * + * @return Current baseline in ECEF x or NED north component in mm. + */ +static inline int32_t mavlink_msg_gps2_rtk_get_baseline_a_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field baseline_b_mm from gps2_rtk message + * + * @return Current baseline in ECEF y or NED east component in mm. + */ +static inline int32_t mavlink_msg_gps2_rtk_get_baseline_b_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field baseline_c_mm from gps2_rtk message + * + * @return Current baseline in ECEF z or NED down component in mm. + */ +static inline int32_t mavlink_msg_gps2_rtk_get_baseline_c_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field accuracy from gps2_rtk message + * + * @return Current estimate of baseline accuracy. + */ +static inline uint32_t mavlink_msg_gps2_rtk_get_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Get field iar_num_hypotheses from gps2_rtk message + * + * @return Current number of integer ambiguity hypotheses. + */ +static inline int32_t mavlink_msg_gps2_rtk_get_iar_num_hypotheses(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Decode a gps2_rtk message into a struct + * + * @param msg The message to decode + * @param gps2_rtk C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps2_rtk_decode(const mavlink_message_t* msg, mavlink_gps2_rtk_t* gps2_rtk) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps2_rtk->time_last_baseline_ms = mavlink_msg_gps2_rtk_get_time_last_baseline_ms(msg); + gps2_rtk->tow = mavlink_msg_gps2_rtk_get_tow(msg); + gps2_rtk->baseline_a_mm = mavlink_msg_gps2_rtk_get_baseline_a_mm(msg); + gps2_rtk->baseline_b_mm = mavlink_msg_gps2_rtk_get_baseline_b_mm(msg); + gps2_rtk->baseline_c_mm = mavlink_msg_gps2_rtk_get_baseline_c_mm(msg); + gps2_rtk->accuracy = mavlink_msg_gps2_rtk_get_accuracy(msg); + gps2_rtk->iar_num_hypotheses = mavlink_msg_gps2_rtk_get_iar_num_hypotheses(msg); + gps2_rtk->wn = mavlink_msg_gps2_rtk_get_wn(msg); + gps2_rtk->rtk_receiver_id = mavlink_msg_gps2_rtk_get_rtk_receiver_id(msg); + gps2_rtk->rtk_health = mavlink_msg_gps2_rtk_get_rtk_health(msg); + gps2_rtk->rtk_rate = mavlink_msg_gps2_rtk_get_rtk_rate(msg); + gps2_rtk->nsats = mavlink_msg_gps2_rtk_get_nsats(msg); + gps2_rtk->baseline_coords_type = mavlink_msg_gps2_rtk_get_baseline_coords_type(msg); +#else + memcpy(gps2_rtk, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h index b53996206..9ab81fcfc 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h @@ -12,7 +12,7 @@ typedef struct __mavlink_gps_raw_int_t uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 } mavlink_gps_raw_int_t; @@ -48,7 +48,7 @@ typedef struct __mavlink_gps_raw_int_t * @param msg The MAVLink message to compress the data into * * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) @@ -107,7 +107,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) @@ -192,7 +192,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, ui * @param chan MAVLink channel to send the message * * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) @@ -313,7 +313,7 @@ static inline uint64_t mavlink_msg_gps_raw_int_get_time_usec(const mavlink_messa /** * @brief Get field fix_type from gps_raw_int message * - * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @return 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_rtk.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_rtk.h new file mode 100644 index 000000000..31052ec25 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_rtk.h @@ -0,0 +1,497 @@ +// MESSAGE GPS_RTK PACKING + +#define MAVLINK_MSG_ID_GPS_RTK 127 + +typedef struct __mavlink_gps_rtk_t +{ + uint32_t time_last_baseline_ms; ///< Time since boot of last baseline message received in ms. + uint32_t tow; ///< GPS Time of Week of last baseline + int32_t baseline_a_mm; ///< Current baseline in ECEF x or NED north component in mm. + int32_t baseline_b_mm; ///< Current baseline in ECEF y or NED east component in mm. + int32_t baseline_c_mm; ///< Current baseline in ECEF z or NED down component in mm. + uint32_t accuracy; ///< Current estimate of baseline accuracy. + int32_t iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses. + uint16_t wn; ///< GPS Week Number of last baseline + uint8_t rtk_receiver_id; ///< Identification of connected RTK receiver. + uint8_t rtk_health; ///< GPS-specific health report for RTK data. + uint8_t rtk_rate; ///< Rate of baseline messages being received by GPS, in HZ + uint8_t nsats; ///< Current number of sats used for RTK calculation. + uint8_t baseline_coords_type; ///< Coordinate system of baseline. 0 == ECEF, 1 == NED +} mavlink_gps_rtk_t; + +#define MAVLINK_MSG_ID_GPS_RTK_LEN 35 +#define MAVLINK_MSG_ID_127_LEN 35 + +#define MAVLINK_MSG_ID_GPS_RTK_CRC 25 +#define MAVLINK_MSG_ID_127_CRC 25 + + + +#define MAVLINK_MESSAGE_INFO_GPS_RTK { \ + "GPS_RTK", \ + 13, \ + { { "time_last_baseline_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gps_rtk_t, time_last_baseline_ms) }, \ + { "tow", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gps_rtk_t, tow) }, \ + { "baseline_a_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_rtk_t, baseline_a_mm) }, \ + { "baseline_b_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps_rtk_t, baseline_b_mm) }, \ + { "baseline_c_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps_rtk_t, baseline_c_mm) }, \ + { "accuracy", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps_rtk_t, accuracy) }, \ + { "iar_num_hypotheses", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_gps_rtk_t, iar_num_hypotheses) }, \ + { "wn", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps_rtk_t, wn) }, \ + { "rtk_receiver_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gps_rtk_t, rtk_receiver_id) }, \ + { "rtk_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gps_rtk_t, rtk_health) }, \ + { "rtk_rate", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps_rtk_t, rtk_rate) }, \ + { "nsats", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps_rtk_t, nsats) }, \ + { "baseline_coords_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps_rtk_t, baseline_coords_type) }, \ + } \ +} + + +/** + * @brief Pack a gps_rtk message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_rtk_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#else + mavlink_gps_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +} + +/** + * @brief Pack a gps_rtk message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_rtk_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_last_baseline_ms,uint8_t rtk_receiver_id,uint16_t wn,uint32_t tow,uint8_t rtk_health,uint8_t rtk_rate,uint8_t nsats,uint8_t baseline_coords_type,int32_t baseline_a_mm,int32_t baseline_b_mm,int32_t baseline_c_mm,uint32_t accuracy,int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#else + mavlink_gps_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +} + +/** + * @brief Encode a gps_rtk struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_rtk_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_rtk_t* gps_rtk) +{ + return mavlink_msg_gps_rtk_pack(system_id, component_id, msg, gps_rtk->time_last_baseline_ms, gps_rtk->rtk_receiver_id, gps_rtk->wn, gps_rtk->tow, gps_rtk->rtk_health, gps_rtk->rtk_rate, gps_rtk->nsats, gps_rtk->baseline_coords_type, gps_rtk->baseline_a_mm, gps_rtk->baseline_b_mm, gps_rtk->baseline_c_mm, gps_rtk->accuracy, gps_rtk->iar_num_hypotheses); +} + +/** + * @brief Encode a gps_rtk struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_rtk_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_rtk_t* gps_rtk) +{ + return mavlink_msg_gps_rtk_pack_chan(system_id, component_id, chan, msg, gps_rtk->time_last_baseline_ms, gps_rtk->rtk_receiver_id, gps_rtk->wn, gps_rtk->tow, gps_rtk->rtk_health, gps_rtk->rtk_rate, gps_rtk->nsats, gps_rtk->baseline_coords_type, gps_rtk->baseline_a_mm, gps_rtk->baseline_b_mm, gps_rtk->baseline_c_mm, gps_rtk->accuracy, gps_rtk->iar_num_hypotheses); +} + +/** + * @brief Send a gps_rtk message + * @param chan MAVLink channel to send the message + * + * @param time_last_baseline_ms Time since boot of last baseline message received in ms. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate Rate of baseline messages being received by GPS, in HZ + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline. 0 == ECEF, 1 == NED + * @param baseline_a_mm Current baseline in ECEF x or NED north component in mm. + * @param baseline_b_mm Current baseline in ECEF y or NED east component in mm. + * @param baseline_c_mm Current baseline in ECEF z or NED down component in mm. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_rtk_send(mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +#else + mavlink_gps_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_GPS_RTK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_rtk_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +#else + mavlink_gps_rtk_t *packet = (mavlink_gps_rtk_t *)msgbuf; + packet->time_last_baseline_ms = time_last_baseline_ms; + packet->tow = tow; + packet->baseline_a_mm = baseline_a_mm; + packet->baseline_b_mm = baseline_b_mm; + packet->baseline_c_mm = baseline_c_mm; + packet->accuracy = accuracy; + packet->iar_num_hypotheses = iar_num_hypotheses; + packet->wn = wn; + packet->rtk_receiver_id = rtk_receiver_id; + packet->rtk_health = rtk_health; + packet->rtk_rate = rtk_rate; + packet->nsats = nsats; + packet->baseline_coords_type = baseline_coords_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE GPS_RTK UNPACKING + + +/** + * @brief Get field time_last_baseline_ms from gps_rtk message + * + * @return Time since boot of last baseline message received in ms. + */ +static inline uint32_t mavlink_msg_gps_rtk_get_time_last_baseline_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field rtk_receiver_id from gps_rtk message + * + * @return Identification of connected RTK receiver. + */ +static inline uint8_t mavlink_msg_gps_rtk_get_rtk_receiver_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field wn from gps_rtk message + * + * @return GPS Week Number of last baseline + */ +static inline uint16_t mavlink_msg_gps_rtk_get_wn(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field tow from gps_rtk message + * + * @return GPS Time of Week of last baseline + */ +static inline uint32_t mavlink_msg_gps_rtk_get_tow(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field rtk_health from gps_rtk message + * + * @return GPS-specific health report for RTK data. + */ +static inline uint8_t mavlink_msg_gps_rtk_get_rtk_health(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field rtk_rate from gps_rtk message + * + * @return Rate of baseline messages being received by GPS, in HZ + */ +static inline uint8_t mavlink_msg_gps_rtk_get_rtk_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field nsats from gps_rtk message + * + * @return Current number of sats used for RTK calculation. + */ +static inline uint8_t mavlink_msg_gps_rtk_get_nsats(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field baseline_coords_type from gps_rtk message + * + * @return Coordinate system of baseline. 0 == ECEF, 1 == NED + */ +static inline uint8_t mavlink_msg_gps_rtk_get_baseline_coords_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field baseline_a_mm from gps_rtk message + * + * @return Current baseline in ECEF x or NED north component in mm. + */ +static inline int32_t mavlink_msg_gps_rtk_get_baseline_a_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field baseline_b_mm from gps_rtk message + * + * @return Current baseline in ECEF y or NED east component in mm. + */ +static inline int32_t mavlink_msg_gps_rtk_get_baseline_b_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field baseline_c_mm from gps_rtk message + * + * @return Current baseline in ECEF z or NED down component in mm. + */ +static inline int32_t mavlink_msg_gps_rtk_get_baseline_c_mm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field accuracy from gps_rtk message + * + * @return Current estimate of baseline accuracy. + */ +static inline uint32_t mavlink_msg_gps_rtk_get_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Get field iar_num_hypotheses from gps_rtk message + * + * @return Current number of integer ambiguity hypotheses. + */ +static inline int32_t mavlink_msg_gps_rtk_get_iar_num_hypotheses(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Decode a gps_rtk message into a struct + * + * @param msg The message to decode + * @param gps_rtk C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_rtk_decode(const mavlink_message_t* msg, mavlink_gps_rtk_t* gps_rtk) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_rtk->time_last_baseline_ms = mavlink_msg_gps_rtk_get_time_last_baseline_ms(msg); + gps_rtk->tow = mavlink_msg_gps_rtk_get_tow(msg); + gps_rtk->baseline_a_mm = mavlink_msg_gps_rtk_get_baseline_a_mm(msg); + gps_rtk->baseline_b_mm = mavlink_msg_gps_rtk_get_baseline_b_mm(msg); + gps_rtk->baseline_c_mm = mavlink_msg_gps_rtk_get_baseline_c_mm(msg); + gps_rtk->accuracy = mavlink_msg_gps_rtk_get_accuracy(msg); + gps_rtk->iar_num_hypotheses = mavlink_msg_gps_rtk_get_iar_num_hypotheses(msg); + gps_rtk->wn = mavlink_msg_gps_rtk_get_wn(msg); + gps_rtk->rtk_receiver_id = mavlink_msg_gps_rtk_get_rtk_receiver_id(msg); + gps_rtk->rtk_health = mavlink_msg_gps_rtk_get_rtk_health(msg); + gps_rtk->rtk_rate = mavlink_msg_gps_rtk_get_rtk_rate(msg); + gps_rtk->nsats = mavlink_msg_gps_rtk_get_nsats(msg); + gps_rtk->baseline_coords_type = mavlink_msg_gps_rtk_get_baseline_coords_type(msg); +#else + memcpy(gps_rtk, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_ned_position_setpoint_external.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_ned_position_setpoint_external.h index 6e60578ce..b4f540275 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_ned_position_setpoint_external.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_ned_position_setpoint_external.h @@ -11,10 +11,10 @@ typedef struct __mavlink_local_ned_position_setpoint_external_t float vx; ///< X velocity in NED frame in meter / s float vy; ///< Y velocity in NED frame in meter / s float vz; ///< Z velocity in NED frame in meter / s - float ax; ///< X acceleration in NED frame in meter / s^2 - float ay; ///< Y acceleration in NED frame in meter / s^2 - float az; ///< Z acceleration in NED frame in meter / s^2 - uint16_t ignore_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + float afx; ///< X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afy; ///< Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + float afz; ///< Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + uint16_t type_mask; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID uint8_t coordinate_frame; ///< Valid options are: MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_OFFSET_NED = 5, MAV_FRAME_BODY_NED = 6, MAV_FRAME_BODY_OFFSET_NED = 7 @@ -23,8 +23,8 @@ typedef struct __mavlink_local_ned_position_setpoint_external_t #define MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN 45 #define MAVLINK_MSG_ID_83_LEN 45 -#define MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_CRC 121 -#define MAVLINK_MSG_ID_83_CRC 121 +#define MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_CRC 211 +#define MAVLINK_MSG_ID_83_CRC 211 @@ -38,10 +38,10 @@ typedef struct __mavlink_local_ned_position_setpoint_external_t { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_ned_position_setpoint_external_t, vx) }, \ { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_ned_position_setpoint_external_t, vy) }, \ { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_ned_position_setpoint_external_t, vz) }, \ - { "ax", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_local_ned_position_setpoint_external_t, ax) }, \ - { "ay", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_local_ned_position_setpoint_external_t, ay) }, \ - { "az", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_local_ned_position_setpoint_external_t, az) }, \ - { "ignore_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_local_ned_position_setpoint_external_t, ignore_mask) }, \ + { "afx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_local_ned_position_setpoint_external_t, afx) }, \ + { "afy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_local_ned_position_setpoint_external_t, afy) }, \ + { "afz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_local_ned_position_setpoint_external_t, afz) }, \ + { "type_mask", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_local_ned_position_setpoint_external_t, type_mask) }, \ { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_local_ned_position_setpoint_external_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_local_ned_position_setpoint_external_t, target_component) }, \ { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_local_ned_position_setpoint_external_t, coordinate_frame) }, \ @@ -59,20 +59,20 @@ typedef struct __mavlink_local_ned_position_setpoint_external_t * @param target_system System ID * @param target_component Component ID * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_OFFSET_NED = 5, MAV_FRAME_BODY_NED = 6, MAV_FRAME_BODY_OFFSET_NED = 7 - * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint * @param x X Position in NED frame in meters * @param y Y Position in NED frame in meters * @param z Z Position in NED frame in meters (note, altitude is negative in NED) * @param vx X velocity in NED frame in meter / s * @param vy Y velocity in NED frame in meter / s * @param vz Z velocity in NED frame in meter / s - * @param ax X acceleration in NED frame in meter / s^2 - * @param ay Y acceleration in NED frame in meter / s^2 - * @param az Z acceleration in NED frame in meter / s^2 + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t ignore_mask, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az) + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN]; @@ -83,10 +83,10 @@ static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_pack(uin _mav_put_float(buf, 16, vx); _mav_put_float(buf, 20, vy); _mav_put_float(buf, 24, vz); - _mav_put_float(buf, 28, ax); - _mav_put_float(buf, 32, ay); - _mav_put_float(buf, 36, az); - _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_uint16_t(buf, 40, type_mask); _mav_put_uint8_t(buf, 42, target_system); _mav_put_uint8_t(buf, 43, target_component); _mav_put_uint8_t(buf, 44, coordinate_frame); @@ -101,10 +101,10 @@ static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_pack(uin packet.vx = vx; packet.vy = vy; packet.vz = vz; - packet.ax = ax; - packet.ay = ay; - packet.az = az; - packet.ignore_mask = ignore_mask; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.type_mask = type_mask; packet.target_system = target_system; packet.target_component = target_component; packet.coordinate_frame = coordinate_frame; @@ -130,21 +130,21 @@ static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_pack(uin * @param target_system System ID * @param target_component Component ID * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_OFFSET_NED = 5, MAV_FRAME_BODY_NED = 6, MAV_FRAME_BODY_OFFSET_NED = 7 - * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint * @param x X Position in NED frame in meters * @param y Y Position in NED frame in meters * @param z Z Position in NED frame in meters (note, altitude is negative in NED) * @param vx X velocity in NED frame in meter / s * @param vy Y velocity in NED frame in meter / s * @param vz Z velocity in NED frame in meter / s - * @param ax X acceleration in NED frame in meter / s^2 - * @param ay Y acceleration in NED frame in meter / s^2 - * @param az Z acceleration in NED frame in meter / s^2 + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t coordinate_frame,uint16_t ignore_mask,float x,float y,float z,float vx,float vy,float vz,float ax,float ay,float az) + uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t coordinate_frame,uint16_t type_mask,float x,float y,float z,float vx,float vy,float vz,float afx,float afy,float afz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN]; @@ -155,10 +155,10 @@ static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_pack_cha _mav_put_float(buf, 16, vx); _mav_put_float(buf, 20, vy); _mav_put_float(buf, 24, vz); - _mav_put_float(buf, 28, ax); - _mav_put_float(buf, 32, ay); - _mav_put_float(buf, 36, az); - _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_uint16_t(buf, 40, type_mask); _mav_put_uint8_t(buf, 42, target_system); _mav_put_uint8_t(buf, 43, target_component); _mav_put_uint8_t(buf, 44, coordinate_frame); @@ -173,10 +173,10 @@ static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_pack_cha packet.vx = vx; packet.vy = vy; packet.vz = vz; - packet.ax = ax; - packet.ay = ay; - packet.az = az; - packet.ignore_mask = ignore_mask; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.type_mask = type_mask; packet.target_system = target_system; packet.target_component = target_component; packet.coordinate_frame = coordinate_frame; @@ -202,7 +202,7 @@ static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_pack_cha */ static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_ned_position_setpoint_external_t* local_ned_position_setpoint_external) { - return mavlink_msg_local_ned_position_setpoint_external_pack(system_id, component_id, msg, local_ned_position_setpoint_external->time_boot_ms, local_ned_position_setpoint_external->target_system, local_ned_position_setpoint_external->target_component, local_ned_position_setpoint_external->coordinate_frame, local_ned_position_setpoint_external->ignore_mask, local_ned_position_setpoint_external->x, local_ned_position_setpoint_external->y, local_ned_position_setpoint_external->z, local_ned_position_setpoint_external->vx, local_ned_position_setpoint_external->vy, local_ned_position_setpoint_external->vz, local_ned_position_setpoint_external->ax, local_ned_position_setpoint_external->ay, local_ned_position_setpoint_external->az); + return mavlink_msg_local_ned_position_setpoint_external_pack(system_id, component_id, msg, local_ned_position_setpoint_external->time_boot_ms, local_ned_position_setpoint_external->target_system, local_ned_position_setpoint_external->target_component, local_ned_position_setpoint_external->coordinate_frame, local_ned_position_setpoint_external->type_mask, local_ned_position_setpoint_external->x, local_ned_position_setpoint_external->y, local_ned_position_setpoint_external->z, local_ned_position_setpoint_external->vx, local_ned_position_setpoint_external->vy, local_ned_position_setpoint_external->vz, local_ned_position_setpoint_external->afx, local_ned_position_setpoint_external->afy, local_ned_position_setpoint_external->afz); } /** @@ -216,7 +216,7 @@ static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_encode(u */ static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_ned_position_setpoint_external_t* local_ned_position_setpoint_external) { - return mavlink_msg_local_ned_position_setpoint_external_pack_chan(system_id, component_id, chan, msg, local_ned_position_setpoint_external->time_boot_ms, local_ned_position_setpoint_external->target_system, local_ned_position_setpoint_external->target_component, local_ned_position_setpoint_external->coordinate_frame, local_ned_position_setpoint_external->ignore_mask, local_ned_position_setpoint_external->x, local_ned_position_setpoint_external->y, local_ned_position_setpoint_external->z, local_ned_position_setpoint_external->vx, local_ned_position_setpoint_external->vy, local_ned_position_setpoint_external->vz, local_ned_position_setpoint_external->ax, local_ned_position_setpoint_external->ay, local_ned_position_setpoint_external->az); + return mavlink_msg_local_ned_position_setpoint_external_pack_chan(system_id, component_id, chan, msg, local_ned_position_setpoint_external->time_boot_ms, local_ned_position_setpoint_external->target_system, local_ned_position_setpoint_external->target_component, local_ned_position_setpoint_external->coordinate_frame, local_ned_position_setpoint_external->type_mask, local_ned_position_setpoint_external->x, local_ned_position_setpoint_external->y, local_ned_position_setpoint_external->z, local_ned_position_setpoint_external->vx, local_ned_position_setpoint_external->vy, local_ned_position_setpoint_external->vz, local_ned_position_setpoint_external->afx, local_ned_position_setpoint_external->afy, local_ned_position_setpoint_external->afz); } /** @@ -227,20 +227,20 @@ static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_encode_c * @param target_system System ID * @param target_component Component ID * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_OFFSET_NED = 5, MAV_FRAME_BODY_NED = 6, MAV_FRAME_BODY_OFFSET_NED = 7 - * @param ignore_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint * @param x X Position in NED frame in meters * @param y Y Position in NED frame in meters * @param z Z Position in NED frame in meters (note, altitude is negative in NED) * @param vx X velocity in NED frame in meter / s * @param vy Y velocity in NED frame in meter / s * @param vz Z velocity in NED frame in meter / s - * @param ax X acceleration in NED frame in meter / s^2 - * @param ay Y acceleration in NED frame in meter / s^2 - * @param az Z acceleration in NED frame in meter / s^2 + * @param afx X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_local_ned_position_setpoint_external_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t ignore_mask, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az) +static inline void mavlink_msg_local_ned_position_setpoint_external_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL_LEN]; @@ -251,10 +251,10 @@ static inline void mavlink_msg_local_ned_position_setpoint_external_send(mavlink _mav_put_float(buf, 16, vx); _mav_put_float(buf, 20, vy); _mav_put_float(buf, 24, vz); - _mav_put_float(buf, 28, ax); - _mav_put_float(buf, 32, ay); - _mav_put_float(buf, 36, az); - _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_uint16_t(buf, 40, type_mask); _mav_put_uint8_t(buf, 42, target_system); _mav_put_uint8_t(buf, 43, target_component); _mav_put_uint8_t(buf, 44, coordinate_frame); @@ -273,10 +273,10 @@ static inline void mavlink_msg_local_ned_position_setpoint_external_send(mavlink packet.vx = vx; packet.vy = vy; packet.vz = vz; - packet.ax = ax; - packet.ay = ay; - packet.az = az; - packet.ignore_mask = ignore_mask; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.type_mask = type_mask; packet.target_system = target_system; packet.target_component = target_component; packet.coordinate_frame = coordinate_frame; @@ -297,7 +297,7 @@ static inline void mavlink_msg_local_ned_position_setpoint_external_send(mavlink is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_local_ned_position_setpoint_external_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t ignore_mask, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az) +static inline void mavlink_msg_local_ned_position_setpoint_external_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -308,10 +308,10 @@ static inline void mavlink_msg_local_ned_position_setpoint_external_send_buf(mav _mav_put_float(buf, 16, vx); _mav_put_float(buf, 20, vy); _mav_put_float(buf, 24, vz); - _mav_put_float(buf, 28, ax); - _mav_put_float(buf, 32, ay); - _mav_put_float(buf, 36, az); - _mav_put_uint16_t(buf, 40, ignore_mask); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_uint16_t(buf, 40, type_mask); _mav_put_uint8_t(buf, 42, target_system); _mav_put_uint8_t(buf, 43, target_component); _mav_put_uint8_t(buf, 44, coordinate_frame); @@ -330,10 +330,10 @@ static inline void mavlink_msg_local_ned_position_setpoint_external_send_buf(mav packet->vx = vx; packet->vy = vy; packet->vz = vz; - packet->ax = ax; - packet->ay = ay; - packet->az = az; - packet->ignore_mask = ignore_mask; + packet->afx = afx; + packet->afy = afy; + packet->afz = afz; + packet->type_mask = type_mask; packet->target_system = target_system; packet->target_component = target_component; packet->coordinate_frame = coordinate_frame; @@ -393,11 +393,11 @@ static inline uint8_t mavlink_msg_local_ned_position_setpoint_external_get_coord } /** - * @brief Get field ignore_mask from local_ned_position_setpoint_external message + * @brief Get field type_mask from local_ned_position_setpoint_external message * - * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates it should follow the commanded attitude and rate and none of the setpoint dimensions should be ignored. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az + * @return Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint */ -static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_get_ignore_mask(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_local_ned_position_setpoint_external_get_type_mask(const mavlink_message_t* msg) { return _MAV_RETURN_uint16_t(msg, 40); } @@ -463,31 +463,31 @@ static inline float mavlink_msg_local_ned_position_setpoint_external_get_vz(cons } /** - * @brief Get field ax from local_ned_position_setpoint_external message + * @brief Get field afx from local_ned_position_setpoint_external message * - * @return X acceleration in NED frame in meter / s^2 + * @return X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N */ -static inline float mavlink_msg_local_ned_position_setpoint_external_get_ax(const mavlink_message_t* msg) +static inline float mavlink_msg_local_ned_position_setpoint_external_get_afx(const mavlink_message_t* msg) { return _MAV_RETURN_float(msg, 28); } /** - * @brief Get field ay from local_ned_position_setpoint_external message + * @brief Get field afy from local_ned_position_setpoint_external message * - * @return Y acceleration in NED frame in meter / s^2 + * @return Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N */ -static inline float mavlink_msg_local_ned_position_setpoint_external_get_ay(const mavlink_message_t* msg) +static inline float mavlink_msg_local_ned_position_setpoint_external_get_afy(const mavlink_message_t* msg) { return _MAV_RETURN_float(msg, 32); } /** - * @brief Get field az from local_ned_position_setpoint_external message + * @brief Get field afz from local_ned_position_setpoint_external message * - * @return Z acceleration in NED frame in meter / s^2 + * @return Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N */ -static inline float mavlink_msg_local_ned_position_setpoint_external_get_az(const mavlink_message_t* msg) +static inline float mavlink_msg_local_ned_position_setpoint_external_get_afz(const mavlink_message_t* msg) { return _MAV_RETURN_float(msg, 36); } @@ -508,10 +508,10 @@ static inline void mavlink_msg_local_ned_position_setpoint_external_decode(const local_ned_position_setpoint_external->vx = mavlink_msg_local_ned_position_setpoint_external_get_vx(msg); local_ned_position_setpoint_external->vy = mavlink_msg_local_ned_position_setpoint_external_get_vy(msg); local_ned_position_setpoint_external->vz = mavlink_msg_local_ned_position_setpoint_external_get_vz(msg); - local_ned_position_setpoint_external->ax = mavlink_msg_local_ned_position_setpoint_external_get_ax(msg); - local_ned_position_setpoint_external->ay = mavlink_msg_local_ned_position_setpoint_external_get_ay(msg); - local_ned_position_setpoint_external->az = mavlink_msg_local_ned_position_setpoint_external_get_az(msg); - local_ned_position_setpoint_external->ignore_mask = mavlink_msg_local_ned_position_setpoint_external_get_ignore_mask(msg); + local_ned_position_setpoint_external->afx = mavlink_msg_local_ned_position_setpoint_external_get_afx(msg); + local_ned_position_setpoint_external->afy = mavlink_msg_local_ned_position_setpoint_external_get_afy(msg); + local_ned_position_setpoint_external->afz = mavlink_msg_local_ned_position_setpoint_external_get_afz(msg); + local_ned_position_setpoint_external->type_mask = mavlink_msg_local_ned_position_setpoint_external_get_type_mask(msg); local_ned_position_setpoint_external->target_system = mavlink_msg_local_ned_position_setpoint_external_get_target_system(msg); local_ned_position_setpoint_external->target_component = mavlink_msg_local_ned_position_setpoint_external_get_target_component(msg); local_ned_position_setpoint_external->coordinate_frame = mavlink_msg_local_ned_position_setpoint_external_get_coordinate_frame(msg); diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h index ccfe74f08..73712781c 100644 --- a/mavlink/include/mavlink/v1.0/common/testsuite.h +++ b/mavlink/include/mavlink/v1.0/common/testsuite.h @@ -3456,10 +3456,10 @@ static void mavlink_test_local_ned_position_setpoint_external(uint8_t system_id, packet1.vx = packet_in.vx; packet1.vy = packet_in.vy; packet1.vz = packet_in.vz; - packet1.ax = packet_in.ax; - packet1.ay = packet_in.ay; - packet1.az = packet_in.az; - packet1.ignore_mask = packet_in.ignore_mask; + packet1.afx = packet_in.afx; + packet1.afy = packet_in.afy; + packet1.afz = packet_in.afz; + packet1.type_mask = packet_in.type_mask; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; packet1.coordinate_frame = packet_in.coordinate_frame; @@ -3472,12 +3472,12 @@ static void mavlink_test_local_ned_position_setpoint_external(uint8_t system_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_local_ned_position_setpoint_external_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.ignore_mask , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz , packet1.ax , packet1.ay , packet1.az ); + mavlink_msg_local_ned_position_setpoint_external_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.type_mask , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz , packet1.afx , packet1.afy , packet1.afz ); mavlink_msg_local_ned_position_setpoint_external_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_local_ned_position_setpoint_external_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.ignore_mask , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz , packet1.ax , packet1.ay , packet1.az ); + mavlink_msg_local_ned_position_setpoint_external_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.type_mask , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz , packet1.afx , packet1.afy , packet1.afz ); mavlink_msg_local_ned_position_setpoint_external_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -3490,7 +3490,7 @@ static void mavlink_test_local_ned_position_setpoint_external(uint8_t system_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_local_ned_position_setpoint_external_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.ignore_mask , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz , packet1.ax , packet1.ay , packet1.az ); + mavlink_msg_local_ned_position_setpoint_external_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.type_mask , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz , packet1.afx , packet1.afy , packet1.afz ); mavlink_msg_local_ned_position_setpoint_external_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -3524,10 +3524,10 @@ static void mavlink_test_global_position_setpoint_external_int(uint8_t system_id packet1.vx = packet_in.vx; packet1.vy = packet_in.vy; packet1.vz = packet_in.vz; - packet1.ax = packet_in.ax; - packet1.ay = packet_in.ay; - packet1.az = packet_in.az; - packet1.ignore_mask = packet_in.ignore_mask; + packet1.afx = packet_in.afx; + packet1.afy = packet_in.afy; + packet1.afz = packet_in.afz; + packet1.type_mask = packet_in.type_mask; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; @@ -3539,12 +3539,12 @@ static void mavlink_test_global_position_setpoint_external_int(uint8_t system_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_global_position_setpoint_external_int_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.ignore_mask , packet1.lat_int , packet1.lon_int , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ax , packet1.ay , packet1.az ); + mavlink_msg_global_position_setpoint_external_int_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.type_mask , packet1.lat_int , packet1.lon_int , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.afx , packet1.afy , packet1.afz ); mavlink_msg_global_position_setpoint_external_int_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_global_position_setpoint_external_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.ignore_mask , packet1.lat_int , packet1.lon_int , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ax , packet1.ay , packet1.az ); + mavlink_msg_global_position_setpoint_external_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.type_mask , packet1.lat_int , packet1.lon_int , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.afx , packet1.afy , packet1.afz ); mavlink_msg_global_position_setpoint_external_int_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -3557,7 +3557,7 @@ static void mavlink_test_global_position_setpoint_external_int(uint8_t system_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_global_position_setpoint_external_int_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.ignore_mask , packet1.lat_int , packet1.lon_int , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ax , packet1.ay , packet1.az ); + mavlink_msg_global_position_setpoint_external_int_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.type_mask , packet1.lat_int , packet1.lon_int , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.afx , packet1.afy , packet1.afz ); mavlink_msg_global_position_setpoint_external_int_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -5331,6 +5331,140 @@ static void mavlink_test_serial_control(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_gps_rtk(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gps_rtk_t packet_in = { + 963497464, + }963497672, + }963497880, + }963498088, + }963498296, + }963498504, + }963498712, + }18691, + }223, + }34, + }101, + }168, + }235, + }; + mavlink_gps_rtk_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_last_baseline_ms = packet_in.time_last_baseline_ms; + packet1.tow = packet_in.tow; + packet1.baseline_a_mm = packet_in.baseline_a_mm; + packet1.baseline_b_mm = packet_in.baseline_b_mm; + packet1.baseline_c_mm = packet_in.baseline_c_mm; + packet1.accuracy = packet_in.accuracy; + packet1.iar_num_hypotheses = packet_in.iar_num_hypotheses; + packet1.wn = packet_in.wn; + packet1.rtk_receiver_id = packet_in.rtk_receiver_id; + packet1.rtk_health = packet_in.rtk_health; + packet1.rtk_rate = packet_in.rtk_rate; + packet1.nsats = packet_in.nsats; + packet1.baseline_coords_type = packet_in.baseline_coords_type; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_rtk_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gps_rtk_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_rtk_pack(system_id, component_id, &msg , packet1.time_last_baseline_ms , packet1.rtk_receiver_id , packet1.wn , packet1.tow , packet1.rtk_health , packet1.rtk_rate , packet1.nsats , packet1.baseline_coords_type , packet1.baseline_a_mm , packet1.baseline_b_mm , packet1.baseline_c_mm , packet1.accuracy , packet1.iar_num_hypotheses ); + mavlink_msg_gps_rtk_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_rtk_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_last_baseline_ms , packet1.rtk_receiver_id , packet1.wn , packet1.tow , packet1.rtk_health , packet1.rtk_rate , packet1.nsats , packet1.baseline_coords_type , packet1.baseline_a_mm , packet1.baseline_b_mm , packet1.baseline_c_mm , packet1.accuracy , packet1.iar_num_hypotheses ); + mavlink_msg_gps_rtk_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i Date: Wed, 2 Jul 2014 16:45:42 +0200 Subject: Fix compile error: rename field in rc_channels.h --- src/modules/uORB/topics/rc_channels.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 829d8e57d..8978de471 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -56,7 +56,7 @@ enum RC_CHANNELS_FUNCTION { RETURN, POSCTL, LOITER, - OFFBOARD_MODE, + OFFBOARD, ACRO, FLAPS, AUX_1, -- cgit v1.2.3 From 76aa871a7125c98e79db4dde42de7863a24a762c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 00:26:33 +0200 Subject: estimator: Fixed body / world matrix initialization and reset --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 9622f7e40..deaaf55fe 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2503,12 +2503,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states - Mat3f DCM; - quat2Tbn(DCM, initQuat); + quat2Tbn(Tbn, initQuat); + Tnb = Tbn.transpose(); Vector3f initMagNED; - initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; - initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; - initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; + initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z; + initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z; + initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z; magstate.q0 = initQuat[0]; magstate.q1 = initQuat[1]; @@ -2521,7 +2521,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) magstate.magYbias = magBias.y; magstate.magZbias = magBias.z; magstate.R_MAG = sq(magMeasurementSigma); - magstate.DCM = DCM; + magstate.DCM = Tbn; // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions -- cgit v1.2.3 From 23164228c84f304326026c9fbda6822f2e99e9b3 Mon Sep 17 00:00:00 2001 From: Kynos Date: Thu, 3 Jul 2014 11:11:40 +0200 Subject: Added NAV-TIMEUTC valid flag defines --- src/drivers/gps/ubx.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 1a9e7cb0c..65f2dd2ba 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -104,6 +104,13 @@ #define UBX_RX_NAV_PVT_FLAGS_PSMSTATE 0x1C /**< psmState (Power Save Mode state (see Power Management)) */ #define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID 0x20 /**< headVehValid (Heading of vehicle is valid) */ +/* RX NAV-TIMEUTC message content details */ +/* Bitfield "valid" masks */ +#define UBX_RX_NAV_TIMEUTC_VALID_VALIDTOW 0x01 /**< validTOW (1 = Valid Time of Week) */ +#define UBX_RX_NAV_TIMEUTC_VALID_VALIDKWN 0x02 /**< validWKN (1 = Valid Week Number) */ +#define UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC 0x04 /**< validUTC (1 = Valid UTC Time) */ +#define UBX_RX_NAV_TIMEUTC_VALID_UTCSTANDARD 0xF0 /**< utcStandard (0..15 = UTC standard identifier) */ + /* TX CFG-PRT message contents */ #define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ #define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ @@ -185,7 +192,7 @@ typedef struct { uint8_t hour; /**< Hour of day, range 0..23 (UTC) */ uint8_t min; /**< Minute of hour, range 0..59 (UTC) */ uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */ - uint8_t valid; /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */ + uint8_t valid; /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */ uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */ int32_t nano; /**< Fraction of second (UTC) [-1e9...1e9 ns] */ uint8_t fixType; /**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */ @@ -223,7 +230,7 @@ typedef struct { uint8_t hour; /**< Hour of day, range 0..23 (UTC) */ uint8_t min; /**< Minute of hour, range 0..59 (UTC) */ uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */ - uint8_t valid; /**< Validity Flags (see ubx documentation) */ + uint8_t valid; /**< Validity Flags (see UBX_RX_NAV_TIMEUTC_VALID_...) */ } ubx_payload_rx_nav_timeutc_t; /* Rx NAV-SVINFO Part 1 */ -- cgit v1.2.3 From 05be2400cb8d38645b86900edde38cbced0d22b4 Mon Sep 17 00:00:00 2001 From: Alexey Luchko Date: Thu, 3 Jul 2014 12:13:56 +0300 Subject: fix misprint `lowvsyslog` --- src/modules/systemlib/err.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 6c0e876d1..998b5ac7d 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -86,7 +86,7 @@ warnerr_core(int errcode, const char *fmt, va_list args) fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC lowsyslog("%s: ", getprogname()); - lowvyslog(fmt, args); + lowvsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) -- cgit v1.2.3 From 7be2e0f13605eb9bb34eb04745a0e7a3936d94e9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 11:18:00 +0200 Subject: Hotfix: Typo in disabled code path --- src/modules/systemlib/err.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 6c0e876d1..60dcc62e0 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -86,7 +86,7 @@ warnerr_core(int errcode, const char *fmt, va_list args) fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC lowsyslog("%s: ", getprogname()); - lowvyslog(fmt, args); + lowsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) -- cgit v1.2.3 From 04cca73baac0b027451a76d50b1f3b365b1feeef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 11:26:26 +0200 Subject: Hotfix of the Hotfix 8) --- src/modules/systemlib/err.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 60dcc62e0..998b5ac7d 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -86,7 +86,7 @@ warnerr_core(int errcode, const char *fmt, va_list args) fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC lowsyslog("%s: ", getprogname()); - lowsyslog(fmt, args); + lowvsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) -- cgit v1.2.3 From d5793d6cbee59779c1f5375fa3276bd0fddbe25e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 12:35:38 +0200 Subject: hk micro pcb quad: change min pwm value --- ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index 42ec150c1..369aa1eb4 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -2,8 +2,9 @@ # # Hobbyking Micro Integrated PCB Quadcopter # -# Anton Babushkin +# Thomas Gubler # +echo "HK Micro PCB Quad" sh /etc/init.d/4001_quad_x @@ -23,5 +24,5 @@ then param set MC_YAWRATE_D 0.0 fi -set PWM_MIN 1100 +set PWM_MIN 1200 set PWM_DISARMED 900 -- cgit v1.2.3 From 2ec635dd1c33ef453818bd9396e3cec42694a30f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 13:30:20 +0200 Subject: Update estimator --- .../ekf_att_pos_estimator/estimator_23states.cpp | 32 +++++++++++----------- .../ekf_att_pos_estimator/estimator_utilities.cpp | 1 + 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 393cf4c5a..973de1382 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1747,7 +1747,7 @@ void AttPosEKF::FuseOptFlow() static float losPred[2]; // Transformation matrix from nav to body axes - Mat3f Tnb; + Mat3f Tnb_local; // Transformation matrix from body to sensor axes // assume camera is aligned with Z body axis plus a misalignment // defined by 3 small angles about X, Y and Z body axis @@ -1764,7 +1764,7 @@ void AttPosEKF::FuseOptFlow() for (uint8_t i = 0; i < n_states; i++) { H_LOS[i] = 0.0f; } - Vector3f velNED; + Vector3f velNED_local; Vector3f relVelSensor; // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. @@ -1786,9 +1786,9 @@ void AttPosEKF::FuseOptFlow() vd = statesAtOptFlowTime[6]; pd = statesAtOptFlowTime[9]; ptd = statesAtOptFlowTime[22]; - velNED.x = vn; - velNED.y = ve; - velNED.z = vd; + velNED_local.x = vn; + velNED_local.y = ve; + velNED_local.z = vd; // calculate rotation from NED to body axes float q00 = sq(q0); @@ -1801,24 +1801,24 @@ void AttPosEKF::FuseOptFlow() float q12 = q1 * q2; float q13 = q1 * q3; float q23 = q2 * q3; - Tnb.x.x = q00 + q11 - q22 - q33; - Tnb.y.y = q00 - q11 + q22 - q33; - Tnb.z.z = q00 - q11 - q22 + q33; - Tnb.y.x = 2*(q12 - q03); - Tnb.z.x = 2*(q13 + q02); - Tnb.x.y = 2*(q12 + q03); - Tnb.z.y = 2*(q23 - q01); - Tnb.x.z = 2*(q13 - q02); - Tnb.y.z = 2*(q23 + q01); + Tnb_local.x.x = q00 + q11 - q22 - q33; + Tnb_local.y.y = q00 - q11 + q22 - q33; + Tnb_local.z.z = q00 - q11 - q22 + q33; + Tnb_local.y.x = 2*(q12 - q03); + Tnb_local.z.x = 2*(q13 + q02); + Tnb_local.x.y = 2*(q12 + q03); + Tnb_local.z.y = 2*(q23 - q01); + Tnb_local.x.z = 2*(q13 - q02); + Tnb_local.y.z = 2*(q23 + q01); // calculate transformation from NED to sensor axes - Tns = Tbs*Tnb; + Tns = Tbs*Tnb_local; // calculate range from ground plain to centre of sensor fov assuming flat earth float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f); // calculate relative velocity in sensor frame - relVelSensor = Tns*velNED; + relVelSensor = Tns*velNED_local; // calculate delta angles in sensor axes Vector3f delAngRel = Tbs*delAng; diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 24168b84c..29a8c8d1e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -8,6 +8,7 @@ #ifdef EKF_DEBUG #include +#include static void ekf_debug_print(const char *fmt, va_list args) -- cgit v1.2.3 From 1cca3ca8a5469e66dbb5bebbe518b55e4af426d8 Mon Sep 17 00:00:00 2001 From: Kynos Date: Thu, 3 Jul 2014 13:33:29 +0200 Subject: Use NAV-PVT with ubx7 and ubx8 modules This replaces NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC. --- src/drivers/gps/gps.cpp | 1 - src/drivers/gps/ubx.cpp | 125 +++++++++++++++++++++---- src/drivers/gps/ubx.h | 10 +- src/modules/mavlink/mavlink_receiver.cpp | 1 - src/modules/uORB/topics/vehicle_gps_position.h | 1 - 5 files changed, 113 insertions(+), 25 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 241e3bdf3..401b65dd4 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -299,7 +299,6 @@ GPS::task_main() _report_gps_pos.alt = (int32_t)1200e3f; _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.s_variance_m_s = 10.0f; - _report_gps_pos.p_variance_m = 10.0f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.9f; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 44434a1df..d0854f5e9 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -95,7 +95,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _got_velned(false), _disable_cmd_last(0), _ack_waiting_msg(0), - _ubx_version(0) + _ubx_version(0), + _use_nav_pvt(false) { decode_init(); } @@ -190,38 +191,45 @@ UBX::configure(unsigned &baudrate) /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ - configure_message_rate(UBX_MSG_NAV_POSLLH, 1); + /* try to set rate for NAV-PVT */ + /* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */ + configure_message_rate(UBX_MSG_NAV_PVT, 1); if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } - - configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); - - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; + _use_nav_pvt = false; + } else { + _use_nav_pvt = true; } + UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); - configure_message_rate(UBX_MSG_NAV_SOL, 1); + if (!_use_nav_pvt) { + configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; - } + configure_message_rate(UBX_MSG_NAV_POSLLH, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } - configure_message_rate(UBX_MSG_NAV_VELNED, 1); + configure_message_rate(UBX_MSG_NAV_SOL, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { - return 1; + configure_message_rate(UBX_MSG_NAV_VELNED, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } } configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0); - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_MON_HW, 1); - if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } @@ -454,11 +462,23 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_HANDLE; // handle by default switch (_rx_msg) { + case UBX_MSG_NAV_PVT: + if ( (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ + && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) /* u-blox 8+ msg format */ + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (!_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + break; + case UBX_MSG_NAV_POSLLH: if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) _rx_state = UBX_RXMSG_ERROR_LENGTH; else if (!_configured) _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead break; case UBX_MSG_NAV_SOL: @@ -466,6 +486,8 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_ERROR_LENGTH; else if (!_configured) _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead break; case UBX_MSG_NAV_TIMEUTC: @@ -473,6 +495,8 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_ERROR_LENGTH; else if (!_configured) _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead break; case UBX_MSG_NAV_SVINFO: @@ -489,6 +513,8 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_ERROR_LENGTH; else if (!_configured) _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead break; case UBX_MSG_MON_VER: @@ -675,6 +701,68 @@ UBX::payload_rx_done(void) // handle message switch (_rx_msg) { + case UBX_MSG_NAV_PVT: + UBX_TRACE_RXMSG("Rx NAV-PVT\n"); + + _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; + _gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV; + + _gps_position->lat = _buf.payload_rx_nav_pvt.lat; + _gps_position->lon = _buf.payload_rx_nav_pvt.lon; + _gps_position->alt = _buf.payload_rx_nav_pvt.hMSL; + + _gps_position->eph = (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f; + _gps_position->epv = (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f; + _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f; + + _gps_position->vel_m_s = (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f; + + _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f; + _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f; + _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f; + _gps_position->vel_ned_valid = true; + + _gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; + + { + /* convert to unix timestamp */ + struct tm timeinfo; + timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900; + timeinfo.tm_mon = _buf.payload_rx_nav_pvt.month - 1; + timeinfo.tm_mday = _buf.payload_rx_nav_pvt.day; + timeinfo.tm_hour = _buf.payload_rx_nav_pvt.hour; + timeinfo.tm_min = _buf.payload_rx_nav_pvt.min; + timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; + time_t epoch = mktime(&timeinfo); + +#ifndef CONFIG_RTC + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; + clock_settime(CLOCK_REALTIME, &ts); +#endif + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f); + } + + _gps_position->timestamp_time = hrt_absolute_time(); + _gps_position->timestamp_velocity = hrt_absolute_time(); + _gps_position->timestamp_variance = hrt_absolute_time(); + _gps_position->timestamp_position = hrt_absolute_time(); + + _rate_count_vel++; + _rate_count_lat_lon++; + + _got_posllh = true; + _got_velned = true; + + ret = 1; + break; + case UBX_MSG_NAV_POSLLH: UBX_TRACE_RXMSG("Rx NAV-POSLLH\n"); @@ -697,7 +785,6 @@ UBX::payload_rx_done(void) _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m - _gps_position->p_variance_m = (float)_buf.payload_rx_nav_sol.pAcc * 1e-2f; // from cm to m _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; _gps_position->timestamp_variance = hrt_absolute_time(); diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 65f2dd2ba..219a5762a 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -183,7 +183,7 @@ typedef struct { uint32_t reserved2; } ubx_payload_rx_nav_sol_t; -/* Rx NAV-PVT */ +/* Rx NAV-PVT (ubx8) */ typedef struct { uint32_t iTOW; /**< GPS Time of Week [ms] */ uint16_t year; /**< Year (UTC)*/ @@ -215,9 +215,11 @@ typedef struct { uint16_t pDOP; /**< Position DOP [0.01] */ uint16_t reserved2; uint32_t reserved3; - int32_t headVeh; /**< Heading of vehicle (2-D) [1e-5 deg] */ - uint32_t reserved4; + int32_t headVeh; /**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */ + uint32_t reserved4; /**< (ubx8+ only) */ } ubx_payload_rx_nav_pvt_t; +#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7 (sizeof(ubx_payload_rx_nav_pvt_t) - 8) +#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8 (sizeof(ubx_payload_rx_nav_pvt_t)) /* Rx NAV-TIMEUTC */ typedef struct { @@ -395,6 +397,7 @@ typedef struct { /* General message and payload buffer union */ typedef union { + ubx_payload_rx_nav_pvt_t payload_rx_nav_pvt; ubx_payload_rx_nav_posllh_t payload_rx_nav_posllh; ubx_payload_rx_nav_sol_t payload_rx_nav_sol; ubx_payload_rx_nav_timeutc_t payload_rx_nav_timeutc; @@ -533,6 +536,7 @@ private: uint16_t _ack_waiting_msg; ubx_buf_t _buf; uint32_t _ubx_version; + bool _use_nav_pvt; }; #endif /* UBX_H_ */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 53a1638a3..6d361052c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -741,7 +741,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.timestamp_variance = timestamp; hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph; hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index dbd59f4f3..80d65cd69 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -61,7 +61,6 @@ struct vehicle_gps_position_s { uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ float c_variance_rad; /**< course accuracy estimate rad */ uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ -- cgit v1.2.3 From 7608628e9d3979814557438df0bd4f676ebe49a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 14:25:46 +0200 Subject: Contributing notes --- CONTRIBUTING.md | 44 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 CONTRIBUTING.md diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 000000000..a6f5fa7a3 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,44 @@ +# Contributing to PX4 Firmware + +We follow the [Github flow](https://guides.github.com/introduction/flow/) development model. + +### Fork the project, then clone your repo + +First [fork and clone](https://help.github.com/articles/fork-a-repo) the project project. + +### Create a feature branch + +*Always* branch off master for new features. + +``` +git checkout -b my_descriptive_branch_name +``` + +### Edit and build the code + +The [developer guide](http://pixhawk.org/dev/start) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://pixhawk.org/dev/code_style) when editing files. + +### Commit your changes + +Always write descriptive commit messages and add a fixes or relates note to them with an [issue number](https://github.com/px4/Firmware/issues) (Github will link these then conveniently) + +**Example:** + +``` +Change how the attitude controller works + +- Fixes rate feed forward +- Allows a local body rate override + +Fixes issue #123 +``` + +### Test your changes + +Since we care about safety, we will regularly ask you for test results. Best is to do a test flight (or bench test where it applies) and upload the logfile from it (on the microSD card in the logs directory) to Google Drive or Dropbox and share the link. + +### Push your changes + +Push changes to your repo and send a [pull request](https://github.com/PX4/Firmware/compare/). + +Make sure to provide some testing feedback and if possible the link to a flight log file. -- cgit v1.2.3 From 74f0301fa1553be85d86f7869d70fdda75b4c7b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 14:46:06 +0200 Subject: Changed branch name suggestion --- CONTRIBUTING.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index a6f5fa7a3..3bf02fbff 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -11,7 +11,7 @@ First [fork and clone](https://help.github.com/articles/fork-a-repo) the project *Always* branch off master for new features. ``` -git checkout -b my_descriptive_branch_name +git checkout -b mydescriptivebranchname ``` ### Edit and build the code -- cgit v1.2.3 From d7394c7ef973e34d87187420444baad6fcf9854b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Jul 2014 19:00:22 +0200 Subject: mavlink: stack size increased --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cf6265f09..f73a58fa2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1601,7 +1601,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1950, + 2700, (main_t)&Mavlink::start_helper, (const char **)argv); -- cgit v1.2.3 From 829a317d23093894c3d8974cce887ab3c19eba08 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 4 Jul 2014 15:49:02 +0200 Subject: navigator: takeoff altitude fixed --- src/modules/navigator/mission.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 2e5047e2c..cf6a764bf 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -326,11 +326,12 @@ Mission::set_mission_items() takeoff_alt += _navigator->get_home_position()->alt; } + /* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_navigator->get_vstatus()->condition_landed) { takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); } else { - takeoff_alt = _navigator->get_home_position()->alt + _param_takeoff_alt.get(); + takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", takeoff_alt - _navigator->get_home_position()->alt); -- cgit v1.2.3 From f9696cc68a2a82937ae1189a06edea167c492e00 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 4 Jul 2014 15:52:45 +0200 Subject: hk micro pcb quad startup script: add comment --- ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index 369aa1eb4..5bd67a9e4 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -1,6 +1,7 @@ #!nsh # # Hobbyking Micro Integrated PCB Quadcopter +# with SimonK ESC firmware and Mystery A1510 motors # # Thomas Gubler # -- cgit v1.2.3 From e97161e96b8de4031746556ab4bd4f96d24a30c8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 4 Jul 2014 15:53:26 +0200 Subject: mavlink: stack size for main thread increased --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 026a4d6c9..8dfa32ce8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2238,7 +2238,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1950, + 2700, (main_t)&Mavlink::start_helper, (const char **)argv); -- cgit v1.2.3 From f91477212894fc6e1de68c429c51974443b6075b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 4 Jul 2014 16:01:10 +0200 Subject: micro pcb quad: remove unnecessary setting --- ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb | 1 - 1 file changed, 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index 5bd67a9e4..99ffd73a5 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -26,4 +26,3 @@ then fi set PWM_MIN 1200 -set PWM_DISARMED 900 -- cgit v1.2.3 From d4eae37e478860a59e21f3caceb3d8fc28f9fa7c Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Fri, 4 Jul 2014 21:00:05 -0400 Subject: Commander: Only consider latest active data link heartbeat for timeout. --- src/modules/commander/commander.cpp | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..699ced1ab 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -767,6 +767,7 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; + hrt_abstime latest_heartbeat = 0; bool status_changed = true; bool param_init_forced = true; @@ -1367,19 +1368,24 @@ int commander_thread_main(int argc, char *argv[]) } /* data link check */ - if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { - /* handle the case where data link was regained */ - if (status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: data link regained"); - status.data_link_lost = false; - status_changed = true; - } + if (telemetry.heartbeat_time >= latest_heartbeat) { + if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where data link was regained */ + if (status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: data link regained"); + status.data_link_lost = false; + status_changed = true; + } - } else { - if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = true; - status_changed = true; + /* Only consider data link with most recent heartbeat */ + latest_heartbeat = telemetry.heartbeat_time; + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + status.data_link_lost = true; + status_changed = true; + } } } -- cgit v1.2.3 From b9299e68d4147845bab9ed99509b3e50b7c94ae1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 5 Jul 2014 13:35:12 -0700 Subject: Compiler warning fixes --- src/drivers/blinkm/blinkm.cpp | 2 +- src/lib/geo/geo.c | 14 ++++++------- src/modules/commander/commander.cpp | 8 ++++---- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 23 +++++++++++----------- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 4 ++-- src/modules/systemlib/cpuload.c | 2 +- src/modules/systemlib/systemlib.c | 3 +++ src/systemcmds/param/param.c | 5 +++-- 8 files changed, 33 insertions(+), 28 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 92e089217..6b14f5945 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -293,7 +293,7 @@ BlinkM::BlinkM(int bus, int blinkm) : safety_sub_fd(-1), num_of_cells(0), detected_cells_runcount(0), - t_led_color({0}), + t_led_color{0}, t_led_blink(0), led_thread_runcount(0), led_interval(1000), diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 212e33ff8..230be9281 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -297,12 +297,12 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do // TO DO - this is messed up and won't compile float start_disp_x = radius * sinf(arc_start_bearing); float start_disp_y = radius * cosf(arc_start_bearing); - float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0f; - float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f; - float lon_end = lon_now + end_disp_x / 111111.0f; - float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f; + float end_disp_x = radius * sinf(_wrapPI((double)(arc_start_bearing + arc_sweep))); + float end_disp_y = radius * cosf(_wrapPI((double)(arc_start_bearing + arc_sweep))); + float lon_start = (float)lon_now + start_disp_x / 111111.0f; + float lat_start = (float)lat_now + start_disp_y * cosf(lat_now) / 111111.0f; + float lon_end = (float)lon_now + end_disp_x / 111111.0f; + float lat_end = (float)lat_now + end_disp_y * cosf(lat_now) / 111111.0f; float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); @@ -319,7 +319,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do } - crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); + crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing); return_value = OK; return return_value; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..3bf3c857d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -472,7 +472,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. // We use an float epsilon delta to test float equality. if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { - mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); + mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1); } else { @@ -634,7 +634,7 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); - char *main_states_str[MAIN_STATE_MAX]; + const char *main_states_str[MAIN_STATE_MAX]; main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; @@ -643,7 +643,7 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[MAIN_STATE_ACRO] = "ACRO"; - char *arming_states_str[ARMING_STATE_MAX]; + const char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[ARMING_STATE_INIT] = "INIT"; arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; arming_states_str[ARMING_STATE_ARMED] = "ARMED"; @@ -652,7 +652,7 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - char *nav_states_str[NAVIGATION_STATE_MAX]; + const char *nav_states_str[NAVIGATION_STATE_MAX]; nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 000c02e3d..a9556f453 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -414,6 +414,17 @@ FixedwingPositionControl::FixedwingPositionControl() : _attitude_sp_pub(-1), _nav_capabilities_pub(-1), + _att(), + _att_sp(), + _nav_capabilities(), + _manual(), + _airspeed(), + _control_mode(), + _global_pos(), + _pos_sp_triplet(), + _sensor_combined(), + _range_finder(), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -433,18 +444,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - _att(), - _att_sp(), - _nav_capabilities(), - _manual(), - _airspeed(), - _control_mode(), - _global_pos(), - _pos_sp_triplet(), - _sensor_combined(), _mTecs(), - _was_pos_control_mode(false), - _range_finder() + _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index e4e405227..c22e60ae0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -56,9 +56,9 @@ class BlockOutputLimiter: public SuperBlock { public: // methods - BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + BlockOutputLimiter(SuperBlock *parent, const char *name, bool fAngularLimit = false) : SuperBlock(parent, name), - _isAngularLimit(isAngularLimit), + _isAngularLimit(fAngularLimit), _min(this, "MIN"), _max(this, "MAX") {}; diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index ccc516f39..7aa2f3a5f 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -67,7 +67,7 @@ __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pT __EXPORT struct system_load_s system_load; -extern FAR struct _TCB *sched_gettcb(pid_t pid); +extern FAR struct tcb_s *sched_gettcb(pid_t pid); void cpuload_initialize_once() { diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 9fff3eb88..90d8dd77c 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -54,6 +54,9 @@ #include "systemlib.h" +// Didn't seem right to include up_internal.h, so direct extern instead. +extern void up_systemreset(void) noreturn_function; + void systemreset(bool to_bootloader) { diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index f8bff2f6f..235ab7186 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -228,8 +229,8 @@ do_show_print(void *arg, param_t param) if (!(arg == NULL)) { /* start search */ - char *ss = search_string; - char *pp = p_name; + const char *ss = search_string; + const char *pp = p_name; bool mismatch = false; /* XXX this comparison is only ok for trailing wildcards */ -- cgit v1.2.3 From 4f06e9bdc9c0c6aa6712797a671b56f034656c41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Jul 2014 15:16:39 +0200 Subject: Move a seldomly used module to test config --- makefiles/config_px4fmu-v2_default.mk | 1 - makefiles/config_px4fmu-v2_test.mk | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 4b6b0a4d2..8e83df391 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -33,7 +33,6 @@ MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm -MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 66b2157ed..66d9efbf8 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -25,6 +25,7 @@ MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/pca8574 +MODULES += drivers/roboclaw MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests -- cgit v1.2.3 From bd5d3ebf70dc9e1aef106b60a840c17824d35b9b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Jul 2014 16:08:37 +0200 Subject: telemetry_statur: use 4 separate topics --- src/modules/commander/commander.cpp | 87 +++++++++++++++++++----------- src/modules/mavlink/mavlink_receiver.cpp | 86 +++++++++++++++-------------- src/modules/sdlog2/sdlog2.c | 31 ++++++----- src/modules/sdlog2/sdlog2_format.h | 8 +++ src/modules/sdlog2/sdlog2_messages.h | 35 +++++++----- src/modules/uORB/objects_common.cpp | 5 +- src/modules/uORB/topics/telemetry_status.h | 14 ++++- 7 files changed, 166 insertions(+), 100 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 699ced1ab..0003ec106 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -767,7 +767,6 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; - hrt_abstime latest_heartbeat = 0; bool status_changed = true; bool param_init_forced = true; @@ -797,10 +796,16 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); - /* Subscribe to telemetry status */ - int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); - struct telemetry_status_s telemetry; - memset(&telemetry, 0, sizeof(telemetry)); + /* Subscribe to telemetry status topics */ + int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; + uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; + bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; + + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + telemetry_last_heartbeat[i] = 0; + telemetry_lost[i] = true; + } /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -882,7 +887,6 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = false; bool main_state_changed = false; bool failsafe_old = false; - bool system_checked = false; while (!thread_should_exit) { @@ -939,15 +943,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } - /* Perform system checks (again) once params are loaded and MAVLink is up. */ - if (!system_checked && mavlink_fd && - (telemetry.heartbeat_time > 0) && - (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { - - (void)rc_calibration_check(mavlink_fd); - system_checked = true; - } - orb_check(sp_man_sub, &updated); if (updated) { @@ -960,10 +955,26 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_check(telemetry_sub, &updated); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + orb_check(telemetry_subs[i], &updated); - if (updated) { - orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry); + if (updated) { + struct telemetry_status_s telemetry; + memset(&telemetry, 0, sizeof(telemetry)); + + orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry); + + /* perform system checks when new telemetry link connected */ + if (mavlink_fd && + telemetry_last_heartbeat[i] == 0 && + telemetry.heartbeat_time > 0 && + hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) { + + (void)rc_calibration_check(mavlink_fd); + } + + telemetry_last_heartbeat[i] = telemetry.heartbeat_time; + } } orb_check(sensor_sub, &updated); @@ -1367,28 +1378,40 @@ int commander_thread_main(int argc, char *argv[]) } } - /* data link check */ - if (telemetry.heartbeat_time >= latest_heartbeat) { - if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* data links check */ + bool have_link = false; + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { /* handle the case where data link was regained */ - if (status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: data link regained"); - status.data_link_lost = false; - status_changed = true; + if (telemetry_lost[i]) { + mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i); + telemetry_lost[i] = false; } - - /* Only consider data link with most recent heartbeat */ - latest_heartbeat = telemetry.heartbeat_time; + have_link = true; } else { - if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = true; - status_changed = true; + if (!telemetry_lost[i]) { + mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i); + telemetry_lost[i] = true; } } } + if (have_link) { + /* handle the case where data link was regained */ + if (status.data_link_lost) { + status.data_link_lost = false; + status_changed = true; + } + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST"); + status.data_link_lost = true; + status_changed = true; + } + } + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6d361052c..60da9c47d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -421,32 +421,35 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { - mavlink_radio_status_t rstatus; - mavlink_msg_radio_status_decode(msg, &rstatus); - - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = hrt_absolute_time(); - tstatus.heartbeat_time = _telemetry_heartbeat_time; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; - tstatus.rssi = rstatus.rssi; - tstatus.remote_rssi = rstatus.remrssi; - tstatus.txbuf = rstatus.txbuf; - tstatus.noise = rstatus.noise; - tstatus.remote_noise = rstatus.remnoise; - tstatus.rxerrors = rstatus.rxerrors; - tstatus.fixed = rstatus.fixed; - - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; + tstatus.rssi = rstatus.rssi; + tstatus.remote_rssi = rstatus.remrssi; + tstatus.txbuf = rstatus.txbuf; + tstatus.noise = rstatus.noise; + tstatus.remote_noise = rstatus.remnoise; + tstatus.rxerrors = rstatus.rxerrors; + tstatus.fixed = rstatus.fixed; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); - } + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + } - /* this means that heartbeats alone won't be published to the radio status no more */ - _radio_status_available = true; + /* this means that heartbeats alone won't be published to the radio status no more */ + _radio_status_available = true; + } } void @@ -475,28 +478,31 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) void MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) { - mavlink_heartbeat_t hb; - mavlink_msg_heartbeat_decode(msg, &hb); + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + mavlink_heartbeat_t hb; + mavlink_msg_heartbeat_decode(msg, &hb); - /* ignore own heartbeats, accept only heartbeats from GCS */ - if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - _telemetry_heartbeat_time = hrt_absolute_time(); + /* ignore own heartbeats, accept only heartbeats from GCS */ + if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { + _telemetry_heartbeat_time = hrt_absolute_time(); - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { + /* if no radio status messages arrive, lets at least publish that heartbeats were received */ + if (!_radio_status_available) { - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); - tstatus.timestamp = _telemetry_heartbeat_time; - tstatus.heartbeat_time = _telemetry_heartbeat_time; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + tstatus.timestamp = _telemetry_heartbeat_time; + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + } } } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 39e5b6c41..0d36fa2c6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -979,7 +979,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GVSP_s log_GVSP; struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; - struct log_TELE_s log_TELE; + struct log_TEL_s log_TEL; struct log_EST0_s log_EST0; struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; @@ -1019,7 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int esc_sub; int global_vel_sp_sub; int battery_sub; - int telemetry_sub; + int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; int range_finder_sub; int estimator_status_sub; int tecs_status_sub; @@ -1049,7 +1049,9 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); - subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + } subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); @@ -1479,16 +1481,19 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TELEMETRY --- */ - if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { - log_msg.msg_type = LOG_TELE_MSG; - log_msg.body.log_TELE.rssi = buf.telemetry.rssi; - log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; - log_msg.body.log_TELE.noise = buf.telemetry.noise; - log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; - log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; - log_msg.body.log_TELE.fixed = buf.telemetry.fixed; - log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; - LOGBUFFER_WRITE_AND_COUNT(TELE); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) { + log_msg.msg_type = LOG_TEL0_MSG + i; + log_msg.body.log_TEL.rssi = buf.telemetry.rssi; + log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TEL.noise = buf.telemetry.noise; + log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TEL.fixed = buf.telemetry.fixed; + log_msg.body.log_TEL.txbuf = buf.telemetry.txbuf; + log_msg.body.log_TEL.heartbeat_time = buf.telemetry.heartbeat_time; + LOGBUFFER_WRITE_AND_COUNT(TEL); + } } /* --- BOTTOM DISTANCE --- */ diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index dc5e6c8bd..aff0e3f48 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -91,6 +91,14 @@ struct log_format_s { .labels = _labels \ } +#define LOG_FORMAT_S(_name, _struct_name, _format, _labels) { \ + .type = LOG_##_name##_MSG, \ + .length = sizeof(struct log_##_struct_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ + } + #define LOG_FORMAT_MSG 0x80 #define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 8c05e87c5..08a87e179 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -276,18 +276,7 @@ struct log_DIST_s { uint8_t flags; }; -/* --- TELE - TELEMETRY STATUS --- */ -#define LOG_TELE_MSG 22 -struct log_TELE_s { - uint8_t rssi; - uint8_t remote_rssi; - uint8_t noise; - uint8_t remote_noise; - uint16_t rxerrors; - uint16_t fixed; - uint8_t txbuf; -}; - +// ID 22 available // ID 23 available /* --- PWR - ONBOARD POWER SYSTEM --- */ @@ -385,6 +374,23 @@ struct log_EST1_s { float s[16]; }; +/* --- TEL0..3 - TELEMETRY STATUS --- */ +#define LOG_TEL0_MSG 34 +#define LOG_TEL1_MSG 35 +#define LOG_TEL2_MSG 36 +#define LOG_TEL3_MSG 37 +struct log_TEL_s { + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; + uint64_t heartbeat_time; +}; + + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -432,7 +438,10 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), - LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + LOG_FORMAT_S(TEL0, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL1, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL2, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL3, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 687fc1d4a..9b118205e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -186,7 +186,10 @@ ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/telemetry_status.h" -ORB_DEFINE(telemetry_status, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_0, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_2, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_3, struct telemetry_status_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index e9e00d76c..c4b99d520 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -72,6 +72,18 @@ struct telemetry_status_s { * @} */ -ORB_DECLARE(telemetry_status); +ORB_DECLARE(telemetry_status_0); +ORB_DECLARE(telemetry_status_1); +ORB_DECLARE(telemetry_status_2); +ORB_DECLARE(telemetry_status_3); + +#define TELEMETRY_STATUS_ORB_ID_NUM 4 + +static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_ID_NUM] = { + ORB_ID(telemetry_status_0), + ORB_ID(telemetry_status_1), + ORB_ID(telemetry_status_2), + ORB_ID(telemetry_status_3), +}; #endif /* TOPIC_TELEMETRY_STATUS_H */ -- cgit v1.2.3 From 54b55c37c7ffec5c340f100d2027e91971967793 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Jul 2014 17:33:50 +0200 Subject: Reduce excessive stack sizes for FW estimation / control apps --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e4f805dc0..5d768b73d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1533,7 +1533,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 6000, + 5000, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 000c02e3d..6c86544fa 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1449,7 +1449,7 @@ FixedwingPositionControl::start() _control_task = task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 4048, + 3500, (main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); -- cgit v1.2.3 From 29eab09912ef38eca75b926d5006c0c59b4082b7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Jul 2014 17:58:21 +0200 Subject: sdlog2: TEL message format fixed --- src/modules/sdlog2/sdlog2_messages.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 08a87e179..87f7f718f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -438,10 +438,10 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), - LOG_FORMAT_S(TEL0, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), - LOG_FORMAT_S(TEL1, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), - LOG_FORMAT_S(TEL2, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), - LOG_FORMAT_S(TEL3, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), -- cgit v1.2.3 From 278aafe939bc20c2882d17b6e21da5ab38d1f400 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 6 Jul 2014 11:06:14 -0700 Subject: Bring everything up to double --- src/lib/geo/geo.c | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 230be9281..e600976ce 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -294,23 +294,21 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do // calculate the position of the start and end points. We should not be doing this often // as this function generally will not be called repeatedly when we are out of the sector. - // TO DO - this is messed up and won't compile - float start_disp_x = radius * sinf(arc_start_bearing); - float start_disp_y = radius * cosf(arc_start_bearing); - float end_disp_x = radius * sinf(_wrapPI((double)(arc_start_bearing + arc_sweep))); - float end_disp_y = radius * cosf(_wrapPI((double)(arc_start_bearing + arc_sweep))); - float lon_start = (float)lon_now + start_disp_x / 111111.0f; - float lat_start = (float)lat_now + start_disp_y * cosf(lat_now) / 111111.0f; - float lon_end = (float)lon_now + end_disp_x / 111111.0f; - float lat_end = (float)lat_now + end_disp_y * cosf(lat_now) / 111111.0f; - float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); - float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + double start_disp_x = (double)radius * sin(arc_start_bearing); + double start_disp_y = (double)radius * cos(arc_start_bearing); + double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep))); + double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep))); + double lon_start = lon_now + start_disp_x / 111111.0; + double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0; + double lon_end = lon_now + end_disp_x / 111111.0; + double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0; + double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); if (dist_to_start < dist_to_end) { crosstrack_error->distance = dist_to_start; crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); - } else { crosstrack_error->past_end = true; crosstrack_error->distance = dist_to_end; -- cgit v1.2.3 From d2a2297a14bf2f4dbb71a6ed20c76617e93b14a2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 6 Jul 2014 11:24:39 -0700 Subject: Fixes #1130 --- src/modules/px4iofirmware/i2c.c | 3 +++ src/modules/px4iofirmware/px4io.h | 4 ++-- src/modules/px4iofirmware/registers.c | 2 +- src/modules/px4iofirmware/sbus.c | 4 ++-- 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 76762c0fc..6d1d1fc2d 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -64,12 +64,15 @@ #define rCCR REG(STM32_I2C_CCR_OFFSET) #define rTRISE REG(STM32_I2C_TRISE_OFFSET) +void i2c_reset(void); static int i2c_interrupt(int irq, void *context); static void i2c_rx_setup(void); static void i2c_tx_setup(void); static void i2c_rx_complete(void); static void i2c_tx_complete(void); +#ifdef DEBUG static void i2c_dump(void); +#endif static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index ca175bfbc..b00a96717 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -219,8 +219,8 @@ extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); -extern bool sbus1_output(uint16_t *values, uint16_t num_values); -extern bool sbus2_output(uint16_t *values, uint16_t num_values); +extern void sbus1_output(uint16_t *values, uint16_t num_values); +extern void sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 50108ea1b..b37259997 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -711,7 +711,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val { #define SELECT_PAGE(_page_name) \ do { \ - *values = &_page_name[0]; \ + *values = (uint16_t *)&_page_name[0]; \ *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \ } while(0) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 70ccab180..0f0414148 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -116,14 +116,14 @@ sbus_init(const char *device) return sbus_fd; } -bool +void sbus1_output(uint16_t *values, uint16_t num_values) { char a = 'A'; write(sbus_fd, &a, 1); } -bool +void sbus2_output(uint16_t *values, uint16_t num_values) { char b = 'B'; -- cgit v1.2.3 From ef6c99c1ab4ebce2378bfeef1813b6e9d01367ed Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 6 Jul 2014 11:40:28 -0700 Subject: Restructure the locking around SPI transfers --- src/drivers/device/spi.cpp | 62 ++++++++++++++++++++++------------------------ src/drivers/device/spi.h | 2 ++ 2 files changed, 32 insertions(+), 32 deletions(-) diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index fed6c312c..57cef34d2 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -133,26 +133,44 @@ SPI::probe() int SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) { - irqstate_t state; + int result; if ((send == nullptr) && (recv == nullptr)) return -EINVAL; + LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode; + /* lock the bus as required */ - if (!up_interrupt_context()) { - switch (locking_mode) { - default: - case LOCK_PREEMPTION: - state = irqsave(); - break; - case LOCK_THREADS: - SPI_LOCK(_dev, true); - break; - case LOCK_NONE: - break; + switch (mode) { + default: + case LOCK_PREEMPTION: + { + irqstate_t state = irqsave(); + result = _transfer(send, recv, len); + irqrestore(state); } + break; + case LOCK_THREADS: + SPI_LOCK(_dev, true); + result = _transfer(send, recv, len); + SPI_LOCK(_dev, false); + break; + case LOCK_NONE: + result = _transfer(send, recv, len); + break; } + return result; +} +void +SPI::set_frequency(uint32_t frequency) +{ + _frequency = frequency; +} + +int +SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ SPI_SETFREQUENCY(_dev, _frequency); SPI_SETMODE(_dev, _mode); SPI_SETBITS(_dev, 8); @@ -164,27 +182,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) /* and clean up */ SPI_SELECT(_dev, _device, false); - if (!up_interrupt_context()) { - switch (locking_mode) { - default: - case LOCK_PREEMPTION: - irqrestore(state); - break; - case LOCK_THREADS: - SPI_LOCK(_dev, false); - break; - case LOCK_NONE: - break; - } - } - return OK; } -void -SPI::set_frequency(uint32_t frequency) -{ - _frequency = frequency; -} - } // namespace device diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 299575dc5..8e943b3f2 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -129,6 +129,8 @@ private: enum spi_mode_e _mode; uint32_t _frequency; struct spi_dev_s *_dev; + + int _transfer(uint8_t *send, uint8_t *recv, unsigned len); }; } // namespace device -- cgit v1.2.3 From 06f08ad04d4cb21047944f350e4a75e88914e1e1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Jul 2014 21:37:26 +0200 Subject: commander: require home position for MISSION, fallback to LOITER --- src/modules/commander/commander.cpp | 7 +++++++ src/modules/commander/state_machine_helper.cpp | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..1da364aa5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1718,6 +1718,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } print_reject_mode(status, "AUTO_MISSION"); + + // fallback to LOITER if home position not set + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } // fallback to POSCTL diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 423ce2f23..7fb2e08db 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -271,7 +271,6 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case MAIN_STATE_AUTO_MISSION: case MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { @@ -279,6 +278,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; + case MAIN_STATE_AUTO_MISSION: case MAIN_STATE_AUTO_RTL: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { -- cgit v1.2.3 From 91537934c4ef33444cf582f8a309443d77b3e575 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 6 Jul 2014 14:58:27 -0700 Subject: Better logging --- src/modules/mavlink/mavlink_ftp.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index ca846a465..b2e6e9d30 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -199,12 +199,18 @@ MavlinkFTP::ErrorCode MavlinkFTP::_workList(Request *req) { auto hdr = req->header(); - DIR *dp = opendir(req->dataAsCString()); + + char dirPath[kMaxDataLength]; + strncpy(dirPath, req->dataAsCString(), kMaxDataLength); + + DIR *dp = opendir(dirPath); if (dp == nullptr) { - printf("FTP: can't open path '%s'\n", req->dataAsCString()); + printf("FTP: can't open path '%s'\n", dirPath); return kErrNotDir; } + + printf("FTP: list %s\n", dirPath); ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -216,6 +222,7 @@ MavlinkFTP::_workList(Request *req) for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { + printf("FTP: list %s readdir_r failure\n", dirPath); errorCode = kErrIO; break; } @@ -251,8 +258,8 @@ MavlinkFTP::_workList(Request *req) // copy the name, which we know will fit strcpy((char *)&hdr->data[offset], entry.d_name); + printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset]); offset += strlen(entry.d_name) + 1; - printf("FTP: list %s\n", entry.d_name); } closedir(dp); -- cgit v1.2.3 From b8ba6400f26e573236d60929283c673fb620f28e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 6 Jul 2014 16:04:30 -0700 Subject: More logging --- src/modules/mavlink/mavlink_ftp.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index b2e6e9d30..2da381a2d 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -210,7 +210,7 @@ MavlinkFTP::_workList(Request *req) return kErrNotDir; } - printf("FTP: list %s\n", dirPath); + printf("FTP: list %s offset %d\n", dirPath, hdr->offset); ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -258,7 +258,7 @@ MavlinkFTP::_workList(Request *req) // copy the name, which we know will fit strcpy((char *)&hdr->data[offset], entry.d_name); - printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset]); + printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); offset += strlen(entry.d_name) + 1; } -- cgit v1.2.3 From 2a79689224a381cf777c555d330f0a2aca8e3d9a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 6 Jul 2014 16:51:06 -0700 Subject: Fix unused variable warnings --- src/modules/commander/commander.cpp | 4 ++-- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 7 +------ src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 2 +- src/modules/mavlink/mavlink_ftp.cpp | 2 +- src/systemcmds/tests/test_mathlib.cpp | 3 +++ 5 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f83c1caf8..3d4a297f7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -773,7 +773,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + rc_calibration_check(mavlink_fd); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -934,7 +934,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; /* re-check RC calibration */ - rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + rc_calibration_check(mavlink_fd); } /* navigation parameters */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3e835cf81..3d6c62434 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -807,12 +807,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements - // XXX re-visit - float baro_altitude = _global_pos.alt; - /* filter speed and altitude for controller */ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); - math::Vector<3> accel_earth = _R_nb * accel_body; float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -945,8 +941,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + /* Calculate altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index fc0a2551c..2e37d166e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -220,7 +220,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector * is running) */ - bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); + limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); /* Write part of the status message */ _status.flightPathAngleSp = flightPathAngleSp; diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index ca846a465..55a472bf6 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -346,7 +346,7 @@ MavlinkFTP::_workWrite(Request *req) MavlinkFTP::ErrorCode MavlinkFTP::_workRemove(Request *req) { - auto hdr = req->header(); + //auto hdr = req->header(); // for now, send error reply return kErrPerm; diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 00c649c75..df3ddb966 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -96,8 +96,11 @@ int test_mathlib(int argc, char *argv[]) TEST_OP("Vector<3> %% Vector<3>", v1 % v2); TEST_OP("Vector<3> length", v1.length()); TEST_OP("Vector<3> length squared", v1.length_squared()); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" TEST_OP("Vector<3> element read", volatile float a = v1(0)); TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); +#pragma GCC diagnostic pop TEST_OP("Vector<3> element write", v1(0) = 1.0f); TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); } -- cgit v1.2.3 From e103f5f8cbb7fdac4916e419a79765f8bd14da6d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 7 Jul 2014 14:59:57 +0200 Subject: change mavlink submodule url --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index 946a1fdcf..c5116c1cb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ [submodule "mavlink/include/mavlink/v1.0"] path = mavlink/include/mavlink/v1.0 - url = git@github.com:mavlink/c_library.git + url = https://github.com/mavlink/c_library.git -- cgit v1.2.3 From 530e70bc4697a7e436a4cc9557ce38e139ee2795 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Jul 2014 15:13:27 +0200 Subject: navigator: forgot to fix conflicts --- src/modules/navigator/navigator.h | 2 +- src/modules/navigator/navigator_main.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ae2e01c8c..bf6e2ea0e 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -113,7 +113,7 @@ public: struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } - + struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c959bf891..1a5ba4c1a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -124,7 +124,6 @@ Navigator::Navigator() : _loiter(this, "LOI"), _rtl(this, "RTL"), _offboard(this, "OFF"), - _update_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD") { -- cgit v1.2.3 From 29bf1fe6fa40968f1cda53c3aa9f4dad3ec25ebb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Jul 2014 15:18:12 +0200 Subject: dataman: added macro for offboard storage selection --- src/modules/dataman/dataman.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index dbace21ef..d625bf985 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -57,6 +57,8 @@ extern "C" { DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; + #define DM_KEY_WAYPOINTS_OFFBOARD(_id) (_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1) + /** The maximum number of instances for each item type */ enum { DM_KEY_SAFE_POINTS_MAX = 8, -- cgit v1.2.3 From a29f7cad395ce53b74500a0dc03214186c679378 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Jul 2014 15:18:54 +0200 Subject: navigator: reject mission if the first waypoint is too far from home --- src/modules/navigator/mission.cpp | 69 +++++++++++++++++++++++++++++++++- src/modules/navigator/mission.h | 10 ++++- src/modules/navigator/mission_params.c | 13 ++++++- 3 files changed, 89 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 291e2edeb..53f0724cd 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -61,6 +61,7 @@ Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _param_onboard_enabled(this, "ONBOARD_EN"), _param_takeoff_alt(this, "TAKEOFF_ALT"), + _param_dist_1wp(this, "DIST_1WP"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -69,8 +70,10 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), + _dist_1wp_ok(false), _need_takeoff(true), _takeoff(false) + { /* load initial params */ updateParams(); @@ -246,6 +249,70 @@ Mission::advance_mission() } } +bool +Mission::check_dist_1wp() +{ + if (_dist_1wp_ok) { + /* always return true after at least one successful check */ + return true; + } + + /* check if first waypoint is not too far from home */ + bool mission_valid = false; + if (_param_dist_1wp.get() > 0.0f) { + if (_navigator->get_vstatus()->condition_home_position_valid) { + struct mission_item_s mission_item; + + /* find first waypoint (with lat/lon) item in datamanager */ + for (int i = 0; i < _offboard_mission.count; i++) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i, + &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { + + /* check only items with valid lat/lon */ + if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + mission_item.nav_cmd == NAV_CMD_TAKEOFF || + mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { + + /* check distance from home to item */ + float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); + + if (dist_to_1wp < _param_dist_1wp.get()) { + _dist_1wp_ok = true; + return true; + + } else { + /* item is too far from home */ + mavlink_log_info(_navigator->get_mavlink_fd(), "first wp is too far from home: %.1fm > %.1fm", dist_to_1wp, _param_dist_1wp.get()); + return false; + } + } + + } else { + /* error reading, mission is invalid */ + mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission"); + return false; + } + } + + /* no waypoints found in mission, then we will not fly far away */ + _dist_1wp_ok = true; + return true; + + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), "no home position"); + return false; + } + + } else { + return true; + } +} + void Mission::set_mission_items() { @@ -266,7 +333,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (read_mission_item(false, true, &_mission_item)) { + } else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 40629b1b2..4da6a1155 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -91,6 +91,12 @@ private: */ void advance_mission(); + /** + * Check distance to first waypoint (with lat/lon) + * @return true only if it's not too far from home (< MIS_DIST_1WP) + */ + bool check_dist_1wp(); + /** * Set new mission items */ @@ -127,8 +133,9 @@ private: */ void publish_mission_result(); - control::BlockParamFloat _param_onboard_enabled; + control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; + control::BlockParamFloat _param_dist_1wp; struct mission_s _onboard_mission; struct mission_s _offboard_mission; @@ -148,6 +155,7 @@ private: } _mission_type; bool _inited; + bool _dist_1wp_ok; MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 8692328db..5cb3782c9 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -58,7 +58,6 @@ */ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); - /** * Enable onboard mission * @@ -67,3 +66,15 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); * @group Mission */ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0); + +/** + * Maximal horizontal distance from home to first waypoint + * + * Failsafe check to prevent running mission stored from previous flight on new place. + * Value < 0 means that check disabled. + * + * @min -1 + * @max 200 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 50); -- cgit v1.2.3 From 68dbf0057a3a80713f79a10669128b7bc9b5cebb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 7 Jul 2014 15:25:32 +0200 Subject: add mavlink submodule check script The script checks if the mavlink submodule is up to date --- Tools/check_submodules.sh | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100755 Tools/check_submodules.sh diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh new file mode 100755 index 000000000..52ea7a146 --- /dev/null +++ b/Tools/check_submodules.sh @@ -0,0 +1,10 @@ +#!/bin/sh +STATUSRETVAL=$(git status --porcelain | grep -i "M mavlink/include/mavlink/v1.0") +if [ "$STATUSRETVAL" == "" ]; then + echo "checked mavlink submodule, correct version found" +else + echo "mavlink sub repo not at correct version. Try 'git submodule update'" + exit 1 +fi + +exit 0 -- cgit v1.2.3 From d79a80e8bed19ebbff333c244c3c90bae39c9181 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Jul 2014 16:57:39 +0200 Subject: really remove mavlink folder --- mavlink/include/mavlink/config.h | 1 - mavlink/include/mavlink/v1.0 | 1 - mavlink/lib/pkgconfig/mavlink.pc | 7 - .../share/mavlink/src/v1.0/pixhawk/pixhawk.pb.cc | 5431 -------------------- mavlink/share/pyshared/pymavlink/.gitignore | 13 - .../pyshared/pymavlink/APM_Mavtest/APM_Mavtest.pde | 55 - mavlink/share/pyshared/pymavlink/README.txt | 4 - .../pyshared/pymavlink/examples/apmsetrate.py | 67 - .../share/pyshared/pymavlink/examples/bwtest.py | 58 - .../pyshared/pymavlink/examples/flightmodes.py | 52 - .../pyshared/pymavlink/examples/flighttime.py | 59 - .../share/pyshared/pymavlink/examples/gpslock.py | 68 - .../share/pyshared/pymavlink/examples/magfit.py | 138 - .../pyshared/pymavlink/examples/magfit_delta.py | 145 - .../pyshared/pymavlink/examples/magfit_gps.py | 159 - .../share/pyshared/pymavlink/examples/magtest.py | 120 - .../share/pyshared/pymavlink/examples/mavgraph.py | 196 - .../pyshared/pymavlink/examples/mavlogdump.py | 67 - .../share/pyshared/pymavlink/examples/mavparms.py | 48 - .../share/pyshared/pymavlink/examples/mavtest.py | 41 - .../share/pyshared/pymavlink/examples/mavtester.py | 43 - .../share/pyshared/pymavlink/examples/mavtogpx.py | 83 - .../share/pyshared/pymavlink/examples/rotmat.py | 269 - .../share/pyshared/pymavlink/examples/sigloss.py | 57 - .../share/pyshared/pymavlink/examples/wptogpx.py | 69 - mavlink/share/pyshared/pymavlink/fgFDM.py | 209 - .../share/pyshared/pymavlink/generator/.gitignore | 1 - .../pymavlink/generator/C/include_v0.9/checksum.h | 89 - .../generator/C/include_v0.9/mavlink_helpers.h | 488 -- .../generator/C/include_v0.9/mavlink_types.h | 300 -- .../pymavlink/generator/C/include_v0.9/protocol.h | 319 -- .../generator/C/include_v0.9/test/mavlink.h | 27 - .../C/include_v0.9/test/mavlink_msg_test_types.h | 610 --- .../pymavlink/generator/C/include_v0.9/test/test.h | 53 - .../generator/C/include_v0.9/test/testsuite.h | 120 - .../generator/C/include_v0.9/test/version.h | 12 - .../pymavlink/generator/C/include_v1.0/checksum.h | 89 - .../generator/C/include_v1.0/mavlink_helpers.h | 507 -- .../C/include_v1.0/mavlink_protobuf_manager.hpp | 377 -- .../generator/C/include_v1.0/mavlink_types.h | 158 - .../generator/C/include_v1.0/pixhawk/pixhawk.pb.h | 3663 ------------- .../pymavlink/generator/C/include_v1.0/protocol.h | 322 -- .../generator/C/include_v1.0/test/mavlink.h | 27 - .../C/include_v1.0/test/mavlink_msg_test_types.h | 610 --- .../pymavlink/generator/C/include_v1.0/test/test.h | 53 - .../generator/C/include_v1.0/test/testsuite.h | 120 - .../generator/C/include_v1.0/test/version.h | 12 - .../generator/C/src_v1.0/pixhawk/pixhawk.pb.cc | 5431 -------------------- .../pymavlink/generator/C/test/posix/.gitignore | 3 - .../pymavlink/generator/C/test/posix/testmav.c | 159 - .../pymavlink/generator/C/test/windows/stdafx.cpp | 8 - .../pymavlink/generator/C/test/windows/stdafx.h | 15 - .../pymavlink/generator/C/test/windows/targetver.h | 8 - .../pymavlink/generator/C/test/windows/testmav.cpp | 154 - .../pymavlink/generator/gen_MatrixPilot.py | 93 - .../share/pyshared/pymavlink/generator/gen_all.py | 44 - .../share/pyshared/pymavlink/generator/gen_all.sh | 12 - .../share/pyshared/pymavlink/generator/mavgen.py | 82 - .../share/pyshared/pymavlink/generator/mavgen_c.py | 581 --- .../pyshared/pymavlink/generator/mavgen_python.py | 455 -- .../share/pyshared/pymavlink/generator/mavparse.py | 372 -- .../pyshared/pymavlink/generator/mavtemplate.py | 121 - .../pyshared/pymavlink/generator/mavtestgen.py | 142 - mavlink/share/pyshared/pymavlink/mavextra.py | 154 - mavlink/share/pyshared/pymavlink/mavlink.py | 4930 ------------------ mavlink/share/pyshared/pymavlink/mavlinkv10.py | 5394 ------------------- mavlink/share/pyshared/pymavlink/mavutil.py | 678 --- mavlink/share/pyshared/pymavlink/mavwp.py | 200 - mavlink/share/pyshared/pymavlink/scanwin32.py | 236 - .../pyshared/pymavlink/tools/images/gtk-quit.gif | Bin 1049 -> 0 bytes .../tools/images/media-playback-pause.gif | Bin 314 -> 0 bytes .../tools/images/media-playback-start.gif | Bin 308 -> 0 bytes .../pymavlink/tools/images/media-playback-stop.gif | Bin 305 -> 0 bytes .../pymavlink/tools/images/media-seek-backward.gif | Bin 319 -> 0 bytes .../pymavlink/tools/images/media-seek-forward.gif | Bin 322 -> 0 bytes .../pyshared/pymavlink/tools/images/player_end.gif | Bin 555 -> 0 bytes .../share/pyshared/pymavlink/tools/mavplayback.py | 246 - 77 files changed, 34935 deletions(-) delete mode 100644 mavlink/include/mavlink/config.h delete mode 160000 mavlink/include/mavlink/v1.0 delete mode 100644 mavlink/lib/pkgconfig/mavlink.pc delete mode 100644 mavlink/share/mavlink/src/v1.0/pixhawk/pixhawk.pb.cc delete mode 100644 mavlink/share/pyshared/pymavlink/.gitignore delete mode 100644 mavlink/share/pyshared/pymavlink/APM_Mavtest/APM_Mavtest.pde delete mode 100644 mavlink/share/pyshared/pymavlink/README.txt delete mode 100644 mavlink/share/pyshared/pymavlink/examples/apmsetrate.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/bwtest.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/flightmodes.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/flighttime.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/gpslock.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/magfit.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/magfit_delta.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/magfit_gps.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/magtest.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/mavgraph.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/mavlogdump.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/mavparms.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/mavtest.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/mavtester.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/mavtogpx.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/rotmat.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/sigloss.py delete mode 100644 mavlink/share/pyshared/pymavlink/examples/wptogpx.py delete mode 100644 mavlink/share/pyshared/pymavlink/fgFDM.py delete mode 100644 mavlink/share/pyshared/pymavlink/generator/.gitignore delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/checksum.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_helpers.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_types.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/protocol.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/mavlink.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/mavlink_msg_test_types.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/test.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/testsuite.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/version.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/checksum.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_protobuf_manager.hpp delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_types.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/pixhawk/pixhawk.pb.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/protocol.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/mavlink.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/mavlink_msg_test_types.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/test.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/testsuite.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/version.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/src_v1.0/pixhawk/pixhawk.pb.cc delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/test/posix/.gitignore delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/test/posix/testmav.c delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.cpp delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/test/windows/targetver.h delete mode 100644 mavlink/share/pyshared/pymavlink/generator/C/test/windows/testmav.cpp delete mode 100644 mavlink/share/pyshared/pymavlink/generator/gen_MatrixPilot.py delete mode 100644 mavlink/share/pyshared/pymavlink/generator/gen_all.py delete mode 100644 mavlink/share/pyshared/pymavlink/generator/gen_all.sh delete mode 100644 mavlink/share/pyshared/pymavlink/generator/mavgen.py delete mode 100644 mavlink/share/pyshared/pymavlink/generator/mavgen_c.py delete mode 100644 mavlink/share/pyshared/pymavlink/generator/mavgen_python.py delete mode 100644 mavlink/share/pyshared/pymavlink/generator/mavparse.py delete mode 100644 mavlink/share/pyshared/pymavlink/generator/mavtemplate.py delete mode 100644 mavlink/share/pyshared/pymavlink/generator/mavtestgen.py delete mode 100644 mavlink/share/pyshared/pymavlink/mavextra.py delete mode 100644 mavlink/share/pyshared/pymavlink/mavlink.py delete mode 100644 mavlink/share/pyshared/pymavlink/mavlinkv10.py delete mode 100644 mavlink/share/pyshared/pymavlink/mavutil.py delete mode 100644 mavlink/share/pyshared/pymavlink/mavwp.py delete mode 100644 mavlink/share/pyshared/pymavlink/scanwin32.py delete mode 100644 mavlink/share/pyshared/pymavlink/tools/images/gtk-quit.gif delete mode 100644 mavlink/share/pyshared/pymavlink/tools/images/media-playback-pause.gif delete mode 100644 mavlink/share/pyshared/pymavlink/tools/images/media-playback-start.gif delete mode 100644 mavlink/share/pyshared/pymavlink/tools/images/media-playback-stop.gif delete mode 100644 mavlink/share/pyshared/pymavlink/tools/images/media-seek-backward.gif delete mode 100644 mavlink/share/pyshared/pymavlink/tools/images/media-seek-forward.gif delete mode 100644 mavlink/share/pyshared/pymavlink/tools/images/player_end.gif delete mode 100644 mavlink/share/pyshared/pymavlink/tools/mavplayback.py diff --git a/mavlink/include/mavlink/config.h b/mavlink/include/mavlink/config.h deleted file mode 100644 index db7db0d7d..000000000 --- a/mavlink/include/mavlink/config.h +++ /dev/null @@ -1 +0,0 @@ -#define MAVLINK_VERSION "1.0.7" diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 deleted file mode 160000 index 45a71d656..000000000 --- a/mavlink/include/mavlink/v1.0 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 45a71d6564bc5c47ed97d620089e17ca48bab73f diff --git a/mavlink/lib/pkgconfig/mavlink.pc b/mavlink/lib/pkgconfig/mavlink.pc deleted file mode 100644 index be9036495..000000000 --- a/mavlink/lib/pkgconfig/mavlink.pc +++ /dev/null @@ -1,7 +0,0 @@ -prefix=/ -exec_prefix=/ - -Name: mavlink -Description: -Version: -Cflags: -I//include/mavlink diff --git a/mavlink/share/mavlink/src/v1.0/pixhawk/pixhawk.pb.cc b/mavlink/share/mavlink/src/v1.0/pixhawk/pixhawk.pb.cc deleted file mode 100644 index e984f512a..000000000 --- a/mavlink/share/mavlink/src/v1.0/pixhawk/pixhawk.pb.cc +++ /dev/null @@ -1,5431 +0,0 @@ -// Generated by the protocol buffer compiler. DO NOT EDIT! - -#define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION -#include "pixhawk.pb.h" - -#include - -#include -#include -#include -#include -#include -#include -// @@protoc_insertion_point(includes) - -namespace px { - -namespace { - -const ::google::protobuf::Descriptor* HeaderInfo_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - HeaderInfo_reflection_ = NULL; -const ::google::protobuf::Descriptor* GLOverlay_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - GLOverlay_reflection_ = NULL; -const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor_ = NULL; -const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor_ = NULL; -const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor_ = NULL; -const ::google::protobuf::Descriptor* Obstacle_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - Obstacle_reflection_ = NULL; -const ::google::protobuf::Descriptor* ObstacleList_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - ObstacleList_reflection_ = NULL; -const ::google::protobuf::Descriptor* ObstacleMap_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - ObstacleMap_reflection_ = NULL; -const ::google::protobuf::Descriptor* Path_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - Path_reflection_ = NULL; -const ::google::protobuf::Descriptor* PointCloudXYZI_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - PointCloudXYZI_reflection_ = NULL; -const ::google::protobuf::Descriptor* PointCloudXYZI_PointXYZI_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - PointCloudXYZI_PointXYZI_reflection_ = NULL; -const ::google::protobuf::Descriptor* PointCloudXYZRGB_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - PointCloudXYZRGB_reflection_ = NULL; -const ::google::protobuf::Descriptor* PointCloudXYZRGB_PointXYZRGB_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - PointCloudXYZRGB_PointXYZRGB_reflection_ = NULL; -const ::google::protobuf::Descriptor* RGBDImage_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - RGBDImage_reflection_ = NULL; -const ::google::protobuf::Descriptor* Waypoint_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - Waypoint_reflection_ = NULL; - -} // namespace - - -void protobuf_AssignDesc_pixhawk_2eproto() { - protobuf_AddDesc_pixhawk_2eproto(); - const ::google::protobuf::FileDescriptor* file = - ::google::protobuf::DescriptorPool::generated_pool()->FindFileByName( - "pixhawk.proto"); - GOOGLE_CHECK(file != NULL); - HeaderInfo_descriptor_ = file->message_type(0); - static const int HeaderInfo_offsets_[3] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, source_sysid_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, source_compid_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, timestamp_), - }; - HeaderInfo_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - HeaderInfo_descriptor_, - HeaderInfo::default_instance_, - HeaderInfo_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(HeaderInfo)); - GLOverlay_descriptor_ = file->message_type(1); - static const int GLOverlay_offsets_[7] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, name_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, coordinateframetype_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_x_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_y_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_z_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, data_), - }; - GLOverlay_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - GLOverlay_descriptor_, - GLOverlay::default_instance_, - GLOverlay_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(GLOverlay)); - GLOverlay_CoordinateFrameType_descriptor_ = GLOverlay_descriptor_->enum_type(0); - GLOverlay_Mode_descriptor_ = GLOverlay_descriptor_->enum_type(1); - GLOverlay_Identifier_descriptor_ = GLOverlay_descriptor_->enum_type(2); - Obstacle_descriptor_ = file->message_type(2); - static const int Obstacle_offsets_[6] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, x_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, y_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, z_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, length_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, width_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, height_), - }; - Obstacle_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - Obstacle_descriptor_, - Obstacle::default_instance_, - Obstacle_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(Obstacle)); - ObstacleList_descriptor_ = file->message_type(3); - static const int ObstacleList_offsets_[2] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, obstacles_), - }; - ObstacleList_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - ObstacleList_descriptor_, - ObstacleList::default_instance_, - ObstacleList_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(ObstacleList)); - ObstacleMap_descriptor_ = file->message_type(4); - static const int ObstacleMap_offsets_[10] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, type_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, resolution_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, rows_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, cols_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, mapr0_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, mapc0_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, arrayr0_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, arrayc0_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, data_), - }; - ObstacleMap_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - ObstacleMap_descriptor_, - ObstacleMap::default_instance_, - ObstacleMap_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(ObstacleMap)); - Path_descriptor_ = file->message_type(5); - static const int Path_offsets_[2] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, waypoints_), - }; - Path_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - Path_descriptor_, - Path::default_instance_, - Path_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(Path)); - PointCloudXYZI_descriptor_ = file->message_type(6); - static const int PointCloudXYZI_offsets_[2] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, points_), - }; - PointCloudXYZI_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - PointCloudXYZI_descriptor_, - PointCloudXYZI::default_instance_, - PointCloudXYZI_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(PointCloudXYZI)); - PointCloudXYZI_PointXYZI_descriptor_ = PointCloudXYZI_descriptor_->nested_type(0); - static const int PointCloudXYZI_PointXYZI_offsets_[4] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, x_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, y_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, z_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, intensity_), - }; - PointCloudXYZI_PointXYZI_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - PointCloudXYZI_PointXYZI_descriptor_, - PointCloudXYZI_PointXYZI::default_instance_, - PointCloudXYZI_PointXYZI_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(PointCloudXYZI_PointXYZI)); - PointCloudXYZRGB_descriptor_ = file->message_type(7); - static const int PointCloudXYZRGB_offsets_[2] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, points_), - }; - PointCloudXYZRGB_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - PointCloudXYZRGB_descriptor_, - PointCloudXYZRGB::default_instance_, - PointCloudXYZRGB_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(PointCloudXYZRGB)); - PointCloudXYZRGB_PointXYZRGB_descriptor_ = PointCloudXYZRGB_descriptor_->nested_type(0); - static const int PointCloudXYZRGB_PointXYZRGB_offsets_[4] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, x_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, y_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, z_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, rgb_), - }; - PointCloudXYZRGB_PointXYZRGB_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - PointCloudXYZRGB_PointXYZRGB_descriptor_, - PointCloudXYZRGB_PointXYZRGB::default_instance_, - PointCloudXYZRGB_PointXYZRGB_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(PointCloudXYZRGB_PointXYZRGB)); - RGBDImage_descriptor_ = file->message_type(8); - static const int RGBDImage_offsets_[21] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, cols_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, rows_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, step1_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, type1_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, imagedata1_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, step2_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, type2_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, imagedata2_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, camera_config_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, camera_type_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, roll_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, pitch_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, yaw_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, lon_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, lat_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, alt_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, ground_x_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, ground_y_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, ground_z_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, camera_matrix_), - }; - RGBDImage_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - RGBDImage_descriptor_, - RGBDImage::default_instance_, - RGBDImage_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(RGBDImage)); - Waypoint_descriptor_ = file->message_type(9); - static const int Waypoint_offsets_[6] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, x_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, y_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, z_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, roll_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, pitch_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, yaw_), - }; - Waypoint_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - Waypoint_descriptor_, - Waypoint::default_instance_, - Waypoint_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(Waypoint)); -} - -namespace { - -GOOGLE_PROTOBUF_DECLARE_ONCE(protobuf_AssignDescriptors_once_); -inline void protobuf_AssignDescriptorsOnce() { - ::google::protobuf::GoogleOnceInit(&protobuf_AssignDescriptors_once_, - &protobuf_AssignDesc_pixhawk_2eproto); -} - -void protobuf_RegisterTypes(const ::std::string&) { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - HeaderInfo_descriptor_, &HeaderInfo::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - GLOverlay_descriptor_, &GLOverlay::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - Obstacle_descriptor_, &Obstacle::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - ObstacleList_descriptor_, &ObstacleList::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - ObstacleMap_descriptor_, &ObstacleMap::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - Path_descriptor_, &Path::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - PointCloudXYZI_descriptor_, &PointCloudXYZI::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - PointCloudXYZI_PointXYZI_descriptor_, &PointCloudXYZI_PointXYZI::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - PointCloudXYZRGB_descriptor_, &PointCloudXYZRGB::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - PointCloudXYZRGB_PointXYZRGB_descriptor_, &PointCloudXYZRGB_PointXYZRGB::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - RGBDImage_descriptor_, &RGBDImage::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - Waypoint_descriptor_, &Waypoint::default_instance()); -} - -} // namespace - -void protobuf_ShutdownFile_pixhawk_2eproto() { - delete HeaderInfo::default_instance_; - delete HeaderInfo_reflection_; - delete GLOverlay::default_instance_; - delete GLOverlay_reflection_; - delete Obstacle::default_instance_; - delete Obstacle_reflection_; - delete ObstacleList::default_instance_; - delete ObstacleList_reflection_; - delete ObstacleMap::default_instance_; - delete ObstacleMap_reflection_; - delete Path::default_instance_; - delete Path_reflection_; - delete PointCloudXYZI::default_instance_; - delete PointCloudXYZI_reflection_; - delete PointCloudXYZI_PointXYZI::default_instance_; - delete PointCloudXYZI_PointXYZI_reflection_; - delete PointCloudXYZRGB::default_instance_; - delete PointCloudXYZRGB_reflection_; - delete PointCloudXYZRGB_PointXYZRGB::default_instance_; - delete PointCloudXYZRGB_PointXYZRGB_reflection_; - delete RGBDImage::default_instance_; - delete RGBDImage_reflection_; - delete Waypoint::default_instance_; - delete Waypoint_reflection_; -} - -void protobuf_AddDesc_pixhawk_2eproto() { - static bool already_here = false; - if (already_here) return; - already_here = true; - GOOGLE_PROTOBUF_VERIFY_VERSION; - - ::google::protobuf::DescriptorPool::InternalAddGeneratedFile( - "\n\rpixhawk.proto\022\002px\"L\n\nHeaderInfo\022\024\n\014sou" - "rce_sysid\030\001 \002(\005\022\025\n\rsource_compid\030\002 \002(\005\022\021" - "\n\ttimestamp\030\003 \002(\001\"\377\004\n\tGLOverlay\022\036\n\006heade" - "r\030\001 \002(\0132\016.px.HeaderInfo\022\014\n\004name\030\002 \001(\t\022>\n" - "\023coordinateFrameType\030\003 \001(\0162!.px.GLOverla" - "y.CoordinateFrameType\022\020\n\010origin_x\030\004 \001(\001\022" - "\020\n\010origin_y\030\005 \001(\001\022\020\n\010origin_z\030\006 \001(\001\022\014\n\004d" - "ata\030\007 \001(\014\",\n\023CoordinateFrameType\022\n\n\006GLOB" - "AL\020\000\022\t\n\005LOCAL\020\001\"\333\001\n\004Mode\022\n\n\006POINTS\020\000\022\t\n\005" - "LINES\020\001\022\016\n\nLINE_STRIP\020\002\022\r\n\tLINE_LOOP\020\003\022\r" - "\n\tTRIANGLES\020\004\022\022\n\016TRIANGLE_STRIP\020\005\022\020\n\014TRI" - "ANGLE_FAN\020\006\022\t\n\005QUADS\020\007\022\016\n\nQUAD_STRIP\020\010\022\013" - "\n\007POLYGON\020\t\022\020\n\014SOLID_CIRCLE\020\n\022\017\n\013WIRE_CI" - "RCLE\020\013\022\016\n\nSOLID_CUBE\020\014\022\r\n\tWIRE_CUBE\020\r\"\263\001" - "\n\nIdentifier\022\007\n\003END\020\016\022\014\n\010VERTEX2F\020\017\022\014\n\010V" - "ERTEX3F\020\020\022\013\n\007ROTATEF\020\021\022\016\n\nTRANSLATEF\020\022\022\n" - "\n\006SCALEF\020\023\022\017\n\013PUSH_MATRIX\020\024\022\016\n\nPOP_MATRI" - "X\020\025\022\013\n\007COLOR3F\020\026\022\013\n\007COLOR4F\020\027\022\r\n\tPOINTSI" - "ZE\020\030\022\r\n\tLINEWIDTH\020\031\"Z\n\010Obstacle\022\t\n\001x\030\001 \001" - "(\002\022\t\n\001y\030\002 \001(\002\022\t\n\001z\030\003 \001(\002\022\016\n\006length\030\004 \001(\002" - "\022\r\n\005width\030\005 \001(\002\022\016\n\006height\030\006 \001(\002\"O\n\014Obsta" - "cleList\022\036\n\006header\030\001 \002(\0132\016.px.HeaderInfo\022" - "\037\n\tobstacles\030\002 \003(\0132\014.px.Obstacle\"\271\001\n\013Obs" - "tacleMap\022\036\n\006header\030\001 \002(\0132\016.px.HeaderInfo" - "\022\014\n\004type\030\002 \002(\005\022\022\n\nresolution\030\003 \001(\002\022\014\n\004ro" - "ws\030\004 \001(\005\022\014\n\004cols\030\005 \001(\005\022\r\n\005mapR0\030\006 \001(\005\022\r\n" - "\005mapC0\030\007 \001(\005\022\017\n\007arrayR0\030\010 \001(\005\022\017\n\007arrayC0" - "\030\t \001(\005\022\014\n\004data\030\n \001(\014\"G\n\004Path\022\036\n\006header\030\001" - " \002(\0132\016.px.HeaderInfo\022\037\n\twaypoints\030\002 \003(\0132" - "\014.px.Waypoint\"\237\001\n\016PointCloudXYZI\022\036\n\006head" - "er\030\001 \002(\0132\016.px.HeaderInfo\022,\n\006points\030\002 \003(\013" - "2\034.px.PointCloudXYZI.PointXYZI\032\?\n\tPointX" - "YZI\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z\030\003 \002(\002\022\021\n\t" - "intensity\030\004 \002(\002\"\241\001\n\020PointCloudXYZRGB\022\036\n\006" - "header\030\001 \002(\0132\016.px.HeaderInfo\0220\n\006points\030\002" - " \003(\0132 .px.PointCloudXYZRGB.PointXYZRGB\032;" - "\n\013PointXYZRGB\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z" - "\030\003 \002(\002\022\013\n\003rgb\030\004 \002(\002\"\365\002\n\tRGBDImage\022\036\n\006hea" - "der\030\001 \002(\0132\016.px.HeaderInfo\022\014\n\004cols\030\002 \002(\r\022" - "\014\n\004rows\030\003 \002(\r\022\r\n\005step1\030\004 \002(\r\022\r\n\005type1\030\005 " - "\002(\r\022\022\n\nimageData1\030\006 \002(\014\022\r\n\005step2\030\007 \002(\r\022\r" - "\n\005type2\030\010 \002(\r\022\022\n\nimageData2\030\t \002(\014\022\025\n\rcam" - "era_config\030\n \001(\r\022\023\n\013camera_type\030\013 \001(\r\022\014\n" - "\004roll\030\014 \001(\002\022\r\n\005pitch\030\r \001(\002\022\013\n\003yaw\030\016 \001(\002\022" - "\013\n\003lon\030\017 \001(\002\022\013\n\003lat\030\020 \001(\002\022\013\n\003alt\030\021 \001(\002\022\020" - "\n\010ground_x\030\022 \001(\002\022\020\n\010ground_y\030\023 \001(\002\022\020\n\010gr" - "ound_z\030\024 \001(\002\022\025\n\rcamera_matrix\030\025 \003(\002\"U\n\010W" - "aypoint\022\t\n\001x\030\001 \002(\001\022\t\n\001y\030\002 \002(\001\022\t\n\001z\030\003 \001(\001" - "\022\014\n\004roll\030\004 \001(\001\022\r\n\005pitch\030\005 \001(\001\022\013\n\003yaw\030\006 \001" - "(\001", 1962); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile( - "pixhawk.proto", &protobuf_RegisterTypes); - HeaderInfo::default_instance_ = new HeaderInfo(); - GLOverlay::default_instance_ = new GLOverlay(); - Obstacle::default_instance_ = new Obstacle(); - ObstacleList::default_instance_ = new ObstacleList(); - ObstacleMap::default_instance_ = new ObstacleMap(); - Path::default_instance_ = new Path(); - PointCloudXYZI::default_instance_ = new PointCloudXYZI(); - PointCloudXYZI_PointXYZI::default_instance_ = new PointCloudXYZI_PointXYZI(); - PointCloudXYZRGB::default_instance_ = new PointCloudXYZRGB(); - PointCloudXYZRGB_PointXYZRGB::default_instance_ = new PointCloudXYZRGB_PointXYZRGB(); - RGBDImage::default_instance_ = new RGBDImage(); - Waypoint::default_instance_ = new Waypoint(); - HeaderInfo::default_instance_->InitAsDefaultInstance(); - GLOverlay::default_instance_->InitAsDefaultInstance(); - Obstacle::default_instance_->InitAsDefaultInstance(); - ObstacleList::default_instance_->InitAsDefaultInstance(); - ObstacleMap::default_instance_->InitAsDefaultInstance(); - Path::default_instance_->InitAsDefaultInstance(); - PointCloudXYZI::default_instance_->InitAsDefaultInstance(); - PointCloudXYZI_PointXYZI::default_instance_->InitAsDefaultInstance(); - PointCloudXYZRGB::default_instance_->InitAsDefaultInstance(); - PointCloudXYZRGB_PointXYZRGB::default_instance_->InitAsDefaultInstance(); - RGBDImage::default_instance_->InitAsDefaultInstance(); - Waypoint::default_instance_->InitAsDefaultInstance(); - ::google::protobuf::internal::OnShutdown(&protobuf_ShutdownFile_pixhawk_2eproto); -} - -// Force AddDescriptors() to be called at static initialization time. -struct StaticDescriptorInitializer_pixhawk_2eproto { - StaticDescriptorInitializer_pixhawk_2eproto() { - protobuf_AddDesc_pixhawk_2eproto(); - } -} static_descriptor_initializer_pixhawk_2eproto_; - - -// =================================================================== - -#ifndef _MSC_VER -const int HeaderInfo::kSourceSysidFieldNumber; -const int HeaderInfo::kSourceCompidFieldNumber; -const int HeaderInfo::kTimestampFieldNumber; -#endif // !_MSC_VER - -HeaderInfo::HeaderInfo() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void HeaderInfo::InitAsDefaultInstance() { -} - -HeaderInfo::HeaderInfo(const HeaderInfo& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void HeaderInfo::SharedCtor() { - _cached_size_ = 0; - source_sysid_ = 0; - source_compid_ = 0; - timestamp_ = 0; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -HeaderInfo::~HeaderInfo() { - SharedDtor(); -} - -void HeaderInfo::SharedDtor() { - if (this != default_instance_) { - } -} - -void HeaderInfo::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* HeaderInfo::descriptor() { - protobuf_AssignDescriptorsOnce(); - return HeaderInfo_descriptor_; -} - -const HeaderInfo& HeaderInfo::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -HeaderInfo* HeaderInfo::default_instance_ = NULL; - -HeaderInfo* HeaderInfo::New() const { - return new HeaderInfo; -} - -void HeaderInfo::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - source_sysid_ = 0; - source_compid_ = 0; - timestamp_ = 0; - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool HeaderInfo::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required int32 source_sysid = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &source_sysid_))); - set_has_source_sysid(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(16)) goto parse_source_compid; - break; - } - - // required int32 source_compid = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_source_compid: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &source_compid_))); - set_has_source_compid(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(25)) goto parse_timestamp; - break; - } - - // required double timestamp = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_timestamp: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, ×tamp_))); - set_has_timestamp(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void HeaderInfo::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required int32 source_sysid = 1; - if (has_source_sysid()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(1, this->source_sysid(), output); - } - - // required int32 source_compid = 2; - if (has_source_compid()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(2, this->source_compid(), output); - } - - // required double timestamp = 3; - if (has_timestamp()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(3, this->timestamp(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* HeaderInfo::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required int32 source_sysid = 1; - if (has_source_sysid()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(1, this->source_sysid(), target); - } - - // required int32 source_compid = 2; - if (has_source_compid()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(2, this->source_compid(), target); - } - - // required double timestamp = 3; - if (has_timestamp()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(3, this->timestamp(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int HeaderInfo::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required int32 source_sysid = 1; - if (has_source_sysid()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->source_sysid()); - } - - // required int32 source_compid = 2; - if (has_source_compid()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->source_compid()); - } - - // required double timestamp = 3; - if (has_timestamp()) { - total_size += 1 + 8; - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void HeaderInfo::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const HeaderInfo* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void HeaderInfo::MergeFrom(const HeaderInfo& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_source_sysid()) { - set_source_sysid(from.source_sysid()); - } - if (from.has_source_compid()) { - set_source_compid(from.source_compid()); - } - if (from.has_timestamp()) { - set_timestamp(from.timestamp()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void HeaderInfo::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void HeaderInfo::CopyFrom(const HeaderInfo& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool HeaderInfo::IsInitialized() const { - if ((_has_bits_[0] & 0x00000007) != 0x00000007) return false; - - return true; -} - -void HeaderInfo::Swap(HeaderInfo* other) { - if (other != this) { - std::swap(source_sysid_, other->source_sysid_); - std::swap(source_compid_, other->source_compid_); - std::swap(timestamp_, other->timestamp_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata HeaderInfo::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = HeaderInfo_descriptor_; - metadata.reflection = HeaderInfo_reflection_; - return metadata; -} - - -// =================================================================== - -const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor() { - protobuf_AssignDescriptorsOnce(); - return GLOverlay_CoordinateFrameType_descriptor_; -} -bool GLOverlay_CoordinateFrameType_IsValid(int value) { - switch(value) { - case 0: - case 1: - return true; - default: - return false; - } -} - -#ifndef _MSC_VER -const GLOverlay_CoordinateFrameType GLOverlay::GLOBAL; -const GLOverlay_CoordinateFrameType GLOverlay::LOCAL; -const GLOverlay_CoordinateFrameType GLOverlay::CoordinateFrameType_MIN; -const GLOverlay_CoordinateFrameType GLOverlay::CoordinateFrameType_MAX; -const int GLOverlay::CoordinateFrameType_ARRAYSIZE; -#endif // _MSC_VER -const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor() { - protobuf_AssignDescriptorsOnce(); - return GLOverlay_Mode_descriptor_; -} -bool GLOverlay_Mode_IsValid(int value) { - switch(value) { - case 0: - case 1: - case 2: - case 3: - case 4: - case 5: - case 6: - case 7: - case 8: - case 9: - case 10: - case 11: - case 12: - case 13: - return true; - default: - return false; - } -} - -#ifndef _MSC_VER -const GLOverlay_Mode GLOverlay::POINTS; -const GLOverlay_Mode GLOverlay::LINES; -const GLOverlay_Mode GLOverlay::LINE_STRIP; -const GLOverlay_Mode GLOverlay::LINE_LOOP; -const GLOverlay_Mode GLOverlay::TRIANGLES; -const GLOverlay_Mode GLOverlay::TRIANGLE_STRIP; -const GLOverlay_Mode GLOverlay::TRIANGLE_FAN; -const GLOverlay_Mode GLOverlay::QUADS; -const GLOverlay_Mode GLOverlay::QUAD_STRIP; -const GLOverlay_Mode GLOverlay::POLYGON; -const GLOverlay_Mode GLOverlay::SOLID_CIRCLE; -const GLOverlay_Mode GLOverlay::WIRE_CIRCLE; -const GLOverlay_Mode GLOverlay::SOLID_CUBE; -const GLOverlay_Mode GLOverlay::WIRE_CUBE; -const GLOverlay_Mode GLOverlay::Mode_MIN; -const GLOverlay_Mode GLOverlay::Mode_MAX; -const int GLOverlay::Mode_ARRAYSIZE; -#endif // _MSC_VER -const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor() { - protobuf_AssignDescriptorsOnce(); - return GLOverlay_Identifier_descriptor_; -} -bool GLOverlay_Identifier_IsValid(int value) { - switch(value) { - case 14: - case 15: - case 16: - case 17: - case 18: - case 19: - case 20: - case 21: - case 22: - case 23: - case 24: - case 25: - return true; - default: - return false; - } -} - -#ifndef _MSC_VER -const GLOverlay_Identifier GLOverlay::END; -const GLOverlay_Identifier GLOverlay::VERTEX2F; -const GLOverlay_Identifier GLOverlay::VERTEX3F; -const GLOverlay_Identifier GLOverlay::ROTATEF; -const GLOverlay_Identifier GLOverlay::TRANSLATEF; -const GLOverlay_Identifier GLOverlay::SCALEF; -const GLOverlay_Identifier GLOverlay::PUSH_MATRIX; -const GLOverlay_Identifier GLOverlay::POP_MATRIX; -const GLOverlay_Identifier GLOverlay::COLOR3F; -const GLOverlay_Identifier GLOverlay::COLOR4F; -const GLOverlay_Identifier GLOverlay::POINTSIZE; -const GLOverlay_Identifier GLOverlay::LINEWIDTH; -const GLOverlay_Identifier GLOverlay::Identifier_MIN; -const GLOverlay_Identifier GLOverlay::Identifier_MAX; -const int GLOverlay::Identifier_ARRAYSIZE; -#endif // _MSC_VER -#ifndef _MSC_VER -const int GLOverlay::kHeaderFieldNumber; -const int GLOverlay::kNameFieldNumber; -const int GLOverlay::kCoordinateFrameTypeFieldNumber; -const int GLOverlay::kOriginXFieldNumber; -const int GLOverlay::kOriginYFieldNumber; -const int GLOverlay::kOriginZFieldNumber; -const int GLOverlay::kDataFieldNumber; -#endif // !_MSC_VER - -GLOverlay::GLOverlay() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void GLOverlay::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -GLOverlay::GLOverlay(const GLOverlay& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void GLOverlay::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - name_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - coordinateframetype_ = 0; - origin_x_ = 0; - origin_y_ = 0; - origin_z_ = 0; - data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -GLOverlay::~GLOverlay() { - SharedDtor(); -} - -void GLOverlay::SharedDtor() { - if (name_ != &::google::protobuf::internal::kEmptyString) { - delete name_; - } - if (data_ != &::google::protobuf::internal::kEmptyString) { - delete data_; - } - if (this != default_instance_) { - delete header_; - } -} - -void GLOverlay::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* GLOverlay::descriptor() { - protobuf_AssignDescriptorsOnce(); - return GLOverlay_descriptor_; -} - -const GLOverlay& GLOverlay::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -GLOverlay* GLOverlay::default_instance_ = NULL; - -GLOverlay* GLOverlay::New() const { - return new GLOverlay; -} - -void GLOverlay::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - if (has_name()) { - if (name_ != &::google::protobuf::internal::kEmptyString) { - name_->clear(); - } - } - coordinateframetype_ = 0; - origin_x_ = 0; - origin_y_ = 0; - origin_z_ = 0; - if (has_data()) { - if (data_ != &::google::protobuf::internal::kEmptyString) { - data_->clear(); - } - } - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool GLOverlay::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_name; - break; - } - - // optional string name = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_name: - DO_(::google::protobuf::internal::WireFormatLite::ReadString( - input, this->mutable_name())); - ::google::protobuf::internal::WireFormat::VerifyUTF8String( - this->name().data(), this->name().length(), - ::google::protobuf::internal::WireFormat::PARSE); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(24)) goto parse_coordinateFrameType; - break; - } - - // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_coordinateFrameType: - int value; - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - int, ::google::protobuf::internal::WireFormatLite::TYPE_ENUM>( - input, &value))); - if (::px::GLOverlay_CoordinateFrameType_IsValid(value)) { - set_coordinateframetype(static_cast< ::px::GLOverlay_CoordinateFrameType >(value)); - } else { - mutable_unknown_fields()->AddVarint(3, value); - } - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(33)) goto parse_origin_x; - break; - } - - // optional double origin_x = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_origin_x: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &origin_x_))); - set_has_origin_x(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(41)) goto parse_origin_y; - break; - } - - // optional double origin_y = 5; - case 5: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_origin_y: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &origin_y_))); - set_has_origin_y(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(49)) goto parse_origin_z; - break; - } - - // optional double origin_z = 6; - case 6: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_origin_z: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &origin_z_))); - set_has_origin_z(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(58)) goto parse_data; - break; - } - - // optional bytes data = 7; - case 7: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_data: - DO_(::google::protobuf::internal::WireFormatLite::ReadBytes( - input, this->mutable_data())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void GLOverlay::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // optional string name = 2; - if (has_name()) { - ::google::protobuf::internal::WireFormat::VerifyUTF8String( - this->name().data(), this->name().length(), - ::google::protobuf::internal::WireFormat::SERIALIZE); - ::google::protobuf::internal::WireFormatLite::WriteString( - 2, this->name(), output); - } - - // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; - if (has_coordinateframetype()) { - ::google::protobuf::internal::WireFormatLite::WriteEnum( - 3, this->coordinateframetype(), output); - } - - // optional double origin_x = 4; - if (has_origin_x()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(4, this->origin_x(), output); - } - - // optional double origin_y = 5; - if (has_origin_y()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(5, this->origin_y(), output); - } - - // optional double origin_z = 6; - if (has_origin_z()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(6, this->origin_z(), output); - } - - // optional bytes data = 7; - if (has_data()) { - ::google::protobuf::internal::WireFormatLite::WriteBytes( - 7, this->data(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* GLOverlay::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // optional string name = 2; - if (has_name()) { - ::google::protobuf::internal::WireFormat::VerifyUTF8String( - this->name().data(), this->name().length(), - ::google::protobuf::internal::WireFormat::SERIALIZE); - target = - ::google::protobuf::internal::WireFormatLite::WriteStringToArray( - 2, this->name(), target); - } - - // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; - if (has_coordinateframetype()) { - target = ::google::protobuf::internal::WireFormatLite::WriteEnumToArray( - 3, this->coordinateframetype(), target); - } - - // optional double origin_x = 4; - if (has_origin_x()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(4, this->origin_x(), target); - } - - // optional double origin_y = 5; - if (has_origin_y()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(5, this->origin_y(), target); - } - - // optional double origin_z = 6; - if (has_origin_z()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(6, this->origin_z(), target); - } - - // optional bytes data = 7; - if (has_data()) { - target = - ::google::protobuf::internal::WireFormatLite::WriteBytesToArray( - 7, this->data(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int GLOverlay::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - // optional string name = 2; - if (has_name()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::StringSize( - this->name()); - } - - // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; - if (has_coordinateframetype()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::EnumSize(this->coordinateframetype()); - } - - // optional double origin_x = 4; - if (has_origin_x()) { - total_size += 1 + 8; - } - - // optional double origin_y = 5; - if (has_origin_y()) { - total_size += 1 + 8; - } - - // optional double origin_z = 6; - if (has_origin_z()) { - total_size += 1 + 8; - } - - // optional bytes data = 7; - if (has_data()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::BytesSize( - this->data()); - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void GLOverlay::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const GLOverlay* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void GLOverlay::MergeFrom(const GLOverlay& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - if (from.has_name()) { - set_name(from.name()); - } - if (from.has_coordinateframetype()) { - set_coordinateframetype(from.coordinateframetype()); - } - if (from.has_origin_x()) { - set_origin_x(from.origin_x()); - } - if (from.has_origin_y()) { - set_origin_y(from.origin_y()); - } - if (from.has_origin_z()) { - set_origin_z(from.origin_z()); - } - if (from.has_data()) { - set_data(from.data()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void GLOverlay::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void GLOverlay::CopyFrom(const GLOverlay& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool GLOverlay::IsInitialized() const { - if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - return true; -} - -void GLOverlay::Swap(GLOverlay* other) { - if (other != this) { - std::swap(header_, other->header_); - std::swap(name_, other->name_); - std::swap(coordinateframetype_, other->coordinateframetype_); - std::swap(origin_x_, other->origin_x_); - std::swap(origin_y_, other->origin_y_); - std::swap(origin_z_, other->origin_z_); - std::swap(data_, other->data_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata GLOverlay::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = GLOverlay_descriptor_; - metadata.reflection = GLOverlay_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int Obstacle::kXFieldNumber; -const int Obstacle::kYFieldNumber; -const int Obstacle::kZFieldNumber; -const int Obstacle::kLengthFieldNumber; -const int Obstacle::kWidthFieldNumber; -const int Obstacle::kHeightFieldNumber; -#endif // !_MSC_VER - -Obstacle::Obstacle() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void Obstacle::InitAsDefaultInstance() { -} - -Obstacle::Obstacle(const Obstacle& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void Obstacle::SharedCtor() { - _cached_size_ = 0; - x_ = 0; - y_ = 0; - z_ = 0; - length_ = 0; - width_ = 0; - height_ = 0; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -Obstacle::~Obstacle() { - SharedDtor(); -} - -void Obstacle::SharedDtor() { - if (this != default_instance_) { - } -} - -void Obstacle::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* Obstacle::descriptor() { - protobuf_AssignDescriptorsOnce(); - return Obstacle_descriptor_; -} - -const Obstacle& Obstacle::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -Obstacle* Obstacle::default_instance_ = NULL; - -Obstacle* Obstacle::New() const { - return new Obstacle; -} - -void Obstacle::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - x_ = 0; - y_ = 0; - z_ = 0; - length_ = 0; - width_ = 0; - height_ = 0; - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool Obstacle::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // optional float x = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &x_))); - set_has_x(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(21)) goto parse_y; - break; - } - - // optional float y = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_y: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &y_))); - set_has_y(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(29)) goto parse_z; - break; - } - - // optional float z = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_z: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &z_))); - set_has_z(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(37)) goto parse_length; - break; - } - - // optional float length = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_length: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &length_))); - set_has_length(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(45)) goto parse_width; - break; - } - - // optional float width = 5; - case 5: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_width: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &width_))); - set_has_width(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(53)) goto parse_height; - break; - } - - // optional float height = 6; - case 6: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_height: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &height_))); - set_has_height(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void Obstacle::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // optional float x = 1; - if (has_x()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output); - } - - // optional float y = 2; - if (has_y()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output); - } - - // optional float z = 3; - if (has_z()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output); - } - - // optional float length = 4; - if (has_length()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->length(), output); - } - - // optional float width = 5; - if (has_width()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(5, this->width(), output); - } - - // optional float height = 6; - if (has_height()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(6, this->height(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* Obstacle::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // optional float x = 1; - if (has_x()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target); - } - - // optional float y = 2; - if (has_y()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target); - } - - // optional float z = 3; - if (has_z()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target); - } - - // optional float length = 4; - if (has_length()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->length(), target); - } - - // optional float width = 5; - if (has_width()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(5, this->width(), target); - } - - // optional float height = 6; - if (has_height()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(6, this->height(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int Obstacle::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // optional float x = 1; - if (has_x()) { - total_size += 1 + 4; - } - - // optional float y = 2; - if (has_y()) { - total_size += 1 + 4; - } - - // optional float z = 3; - if (has_z()) { - total_size += 1 + 4; - } - - // optional float length = 4; - if (has_length()) { - total_size += 1 + 4; - } - - // optional float width = 5; - if (has_width()) { - total_size += 1 + 4; - } - - // optional float height = 6; - if (has_height()) { - total_size += 1 + 4; - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void Obstacle::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const Obstacle* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void Obstacle::MergeFrom(const Obstacle& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_x()) { - set_x(from.x()); - } - if (from.has_y()) { - set_y(from.y()); - } - if (from.has_z()) { - set_z(from.z()); - } - if (from.has_length()) { - set_length(from.length()); - } - if (from.has_width()) { - set_width(from.width()); - } - if (from.has_height()) { - set_height(from.height()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void Obstacle::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void Obstacle::CopyFrom(const Obstacle& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool Obstacle::IsInitialized() const { - - return true; -} - -void Obstacle::Swap(Obstacle* other) { - if (other != this) { - std::swap(x_, other->x_); - std::swap(y_, other->y_); - std::swap(z_, other->z_); - std::swap(length_, other->length_); - std::swap(width_, other->width_); - std::swap(height_, other->height_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata Obstacle::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = Obstacle_descriptor_; - metadata.reflection = Obstacle_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int ObstacleList::kHeaderFieldNumber; -const int ObstacleList::kObstaclesFieldNumber; -#endif // !_MSC_VER - -ObstacleList::ObstacleList() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void ObstacleList::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -ObstacleList::ObstacleList(const ObstacleList& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void ObstacleList::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -ObstacleList::~ObstacleList() { - SharedDtor(); -} - -void ObstacleList::SharedDtor() { - if (this != default_instance_) { - delete header_; - } -} - -void ObstacleList::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* ObstacleList::descriptor() { - protobuf_AssignDescriptorsOnce(); - return ObstacleList_descriptor_; -} - -const ObstacleList& ObstacleList::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -ObstacleList* ObstacleList::default_instance_ = NULL; - -ObstacleList* ObstacleList::New() const { - return new ObstacleList; -} - -void ObstacleList::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - } - obstacles_.Clear(); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool ObstacleList::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_obstacles; - break; - } - - // repeated .px.Obstacle obstacles = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_obstacles: - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, add_obstacles())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_obstacles; - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void ObstacleList::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // repeated .px.Obstacle obstacles = 2; - for (int i = 0; i < this->obstacles_size(); i++) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 2, this->obstacles(i), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* ObstacleList::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // repeated .px.Obstacle obstacles = 2; - for (int i = 0; i < this->obstacles_size(); i++) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 2, this->obstacles(i), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int ObstacleList::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - } - // repeated .px.Obstacle obstacles = 2; - total_size += 1 * this->obstacles_size(); - for (int i = 0; i < this->obstacles_size(); i++) { - total_size += - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->obstacles(i)); - } - - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void ObstacleList::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const ObstacleList* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void ObstacleList::MergeFrom(const ObstacleList& from) { - GOOGLE_CHECK_NE(&from, this); - obstacles_.MergeFrom(from.obstacles_); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void ObstacleList::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void ObstacleList::CopyFrom(const ObstacleList& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool ObstacleList::IsInitialized() const { - if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - return true; -} - -void ObstacleList::Swap(ObstacleList* other) { - if (other != this) { - std::swap(header_, other->header_); - obstacles_.Swap(&other->obstacles_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata ObstacleList::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = ObstacleList_descriptor_; - metadata.reflection = ObstacleList_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int ObstacleMap::kHeaderFieldNumber; -const int ObstacleMap::kTypeFieldNumber; -const int ObstacleMap::kResolutionFieldNumber; -const int ObstacleMap::kRowsFieldNumber; -const int ObstacleMap::kColsFieldNumber; -const int ObstacleMap::kMapR0FieldNumber; -const int ObstacleMap::kMapC0FieldNumber; -const int ObstacleMap::kArrayR0FieldNumber; -const int ObstacleMap::kArrayC0FieldNumber; -const int ObstacleMap::kDataFieldNumber; -#endif // !_MSC_VER - -ObstacleMap::ObstacleMap() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void ObstacleMap::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -ObstacleMap::ObstacleMap(const ObstacleMap& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void ObstacleMap::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - type_ = 0; - resolution_ = 0; - rows_ = 0; - cols_ = 0; - mapr0_ = 0; - mapc0_ = 0; - arrayr0_ = 0; - arrayc0_ = 0; - data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -ObstacleMap::~ObstacleMap() { - SharedDtor(); -} - -void ObstacleMap::SharedDtor() { - if (data_ != &::google::protobuf::internal::kEmptyString) { - delete data_; - } - if (this != default_instance_) { - delete header_; - } -} - -void ObstacleMap::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* ObstacleMap::descriptor() { - protobuf_AssignDescriptorsOnce(); - return ObstacleMap_descriptor_; -} - -const ObstacleMap& ObstacleMap::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -ObstacleMap* ObstacleMap::default_instance_ = NULL; - -ObstacleMap* ObstacleMap::New() const { - return new ObstacleMap; -} - -void ObstacleMap::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - type_ = 0; - resolution_ = 0; - rows_ = 0; - cols_ = 0; - mapr0_ = 0; - mapc0_ = 0; - arrayr0_ = 0; - } - if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) { - arrayc0_ = 0; - if (has_data()) { - if (data_ != &::google::protobuf::internal::kEmptyString) { - data_->clear(); - } - } - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool ObstacleMap::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(16)) goto parse_type; - break; - } - - // required int32 type = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_type: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &type_))); - set_has_type(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(29)) goto parse_resolution; - break; - } - - // optional float resolution = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_resolution: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &resolution_))); - set_has_resolution(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(32)) goto parse_rows; - break; - } - - // optional int32 rows = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_rows: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &rows_))); - set_has_rows(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(40)) goto parse_cols; - break; - } - - // optional int32 cols = 5; - case 5: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_cols: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &cols_))); - set_has_cols(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(48)) goto parse_mapR0; - break; - } - - // optional int32 mapR0 = 6; - case 6: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_mapR0: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &mapr0_))); - set_has_mapr0(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(56)) goto parse_mapC0; - break; - } - - // optional int32 mapC0 = 7; - case 7: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_mapC0: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &mapc0_))); - set_has_mapc0(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(64)) goto parse_arrayR0; - break; - } - - // optional int32 arrayR0 = 8; - case 8: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_arrayR0: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &arrayr0_))); - set_has_arrayr0(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(72)) goto parse_arrayC0; - break; - } - - // optional int32 arrayC0 = 9; - case 9: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_arrayC0: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &arrayc0_))); - set_has_arrayc0(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(82)) goto parse_data; - break; - } - - // optional bytes data = 10; - case 10: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_data: - DO_(::google::protobuf::internal::WireFormatLite::ReadBytes( - input, this->mutable_data())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void ObstacleMap::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // required int32 type = 2; - if (has_type()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(2, this->type(), output); - } - - // optional float resolution = 3; - if (has_resolution()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->resolution(), output); - } - - // optional int32 rows = 4; - if (has_rows()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(4, this->rows(), output); - } - - // optional int32 cols = 5; - if (has_cols()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(5, this->cols(), output); - } - - // optional int32 mapR0 = 6; - if (has_mapr0()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(6, this->mapr0(), output); - } - - // optional int32 mapC0 = 7; - if (has_mapc0()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(7, this->mapc0(), output); - } - - // optional int32 arrayR0 = 8; - if (has_arrayr0()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(8, this->arrayr0(), output); - } - - // optional int32 arrayC0 = 9; - if (has_arrayc0()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(9, this->arrayc0(), output); - } - - // optional bytes data = 10; - if (has_data()) { - ::google::protobuf::internal::WireFormatLite::WriteBytes( - 10, this->data(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* ObstacleMap::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // required int32 type = 2; - if (has_type()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(2, this->type(), target); - } - - // optional float resolution = 3; - if (has_resolution()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->resolution(), target); - } - - // optional int32 rows = 4; - if (has_rows()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(4, this->rows(), target); - } - - // optional int32 cols = 5; - if (has_cols()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(5, this->cols(), target); - } - - // optional int32 mapR0 = 6; - if (has_mapr0()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(6, this->mapr0(), target); - } - - // optional int32 mapC0 = 7; - if (has_mapc0()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(7, this->mapc0(), target); - } - - // optional int32 arrayR0 = 8; - if (has_arrayr0()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(8, this->arrayr0(), target); - } - - // optional int32 arrayC0 = 9; - if (has_arrayc0()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(9, this->arrayc0(), target); - } - - // optional bytes data = 10; - if (has_data()) { - target = - ::google::protobuf::internal::WireFormatLite::WriteBytesToArray( - 10, this->data(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int ObstacleMap::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - // required int32 type = 2; - if (has_type()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->type()); - } - - // optional float resolution = 3; - if (has_resolution()) { - total_size += 1 + 4; - } - - // optional int32 rows = 4; - if (has_rows()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->rows()); - } - - // optional int32 cols = 5; - if (has_cols()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->cols()); - } - - // optional int32 mapR0 = 6; - if (has_mapr0()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->mapr0()); - } - - // optional int32 mapC0 = 7; - if (has_mapc0()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->mapc0()); - } - - // optional int32 arrayR0 = 8; - if (has_arrayr0()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->arrayr0()); - } - - } - if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) { - // optional int32 arrayC0 = 9; - if (has_arrayc0()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->arrayc0()); - } - - // optional bytes data = 10; - if (has_data()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::BytesSize( - this->data()); - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void ObstacleMap::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const ObstacleMap* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void ObstacleMap::MergeFrom(const ObstacleMap& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - if (from.has_type()) { - set_type(from.type()); - } - if (from.has_resolution()) { - set_resolution(from.resolution()); - } - if (from.has_rows()) { - set_rows(from.rows()); - } - if (from.has_cols()) { - set_cols(from.cols()); - } - if (from.has_mapr0()) { - set_mapr0(from.mapr0()); - } - if (from.has_mapc0()) { - set_mapc0(from.mapc0()); - } - if (from.has_arrayr0()) { - set_arrayr0(from.arrayr0()); - } - } - if (from._has_bits_[8 / 32] & (0xffu << (8 % 32))) { - if (from.has_arrayc0()) { - set_arrayc0(from.arrayc0()); - } - if (from.has_data()) { - set_data(from.data()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void ObstacleMap::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void ObstacleMap::CopyFrom(const ObstacleMap& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool ObstacleMap::IsInitialized() const { - if ((_has_bits_[0] & 0x00000003) != 0x00000003) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - return true; -} - -void ObstacleMap::Swap(ObstacleMap* other) { - if (other != this) { - std::swap(header_, other->header_); - std::swap(type_, other->type_); - std::swap(resolution_, other->resolution_); - std::swap(rows_, other->rows_); - std::swap(cols_, other->cols_); - std::swap(mapr0_, other->mapr0_); - std::swap(mapc0_, other->mapc0_); - std::swap(arrayr0_, other->arrayr0_); - std::swap(arrayc0_, other->arrayc0_); - std::swap(data_, other->data_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata ObstacleMap::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = ObstacleMap_descriptor_; - metadata.reflection = ObstacleMap_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int Path::kHeaderFieldNumber; -const int Path::kWaypointsFieldNumber; -#endif // !_MSC_VER - -Path::Path() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void Path::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -Path::Path(const Path& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void Path::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -Path::~Path() { - SharedDtor(); -} - -void Path::SharedDtor() { - if (this != default_instance_) { - delete header_; - } -} - -void Path::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* Path::descriptor() { - protobuf_AssignDescriptorsOnce(); - return Path_descriptor_; -} - -const Path& Path::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -Path* Path::default_instance_ = NULL; - -Path* Path::New() const { - return new Path; -} - -void Path::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - } - waypoints_.Clear(); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool Path::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_waypoints; - break; - } - - // repeated .px.Waypoint waypoints = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_waypoints: - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, add_waypoints())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_waypoints; - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void Path::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // repeated .px.Waypoint waypoints = 2; - for (int i = 0; i < this->waypoints_size(); i++) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 2, this->waypoints(i), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* Path::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // repeated .px.Waypoint waypoints = 2; - for (int i = 0; i < this->waypoints_size(); i++) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 2, this->waypoints(i), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int Path::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - } - // repeated .px.Waypoint waypoints = 2; - total_size += 1 * this->waypoints_size(); - for (int i = 0; i < this->waypoints_size(); i++) { - total_size += - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->waypoints(i)); - } - - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void Path::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const Path* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void Path::MergeFrom(const Path& from) { - GOOGLE_CHECK_NE(&from, this); - waypoints_.MergeFrom(from.waypoints_); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void Path::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void Path::CopyFrom(const Path& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool Path::IsInitialized() const { - if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - for (int i = 0; i < waypoints_size(); i++) { - if (!this->waypoints(i).IsInitialized()) return false; - } - return true; -} - -void Path::Swap(Path* other) { - if (other != this) { - std::swap(header_, other->header_); - waypoints_.Swap(&other->waypoints_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata Path::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = Path_descriptor_; - metadata.reflection = Path_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int PointCloudXYZI_PointXYZI::kXFieldNumber; -const int PointCloudXYZI_PointXYZI::kYFieldNumber; -const int PointCloudXYZI_PointXYZI::kZFieldNumber; -const int PointCloudXYZI_PointXYZI::kIntensityFieldNumber; -#endif // !_MSC_VER - -PointCloudXYZI_PointXYZI::PointCloudXYZI_PointXYZI() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void PointCloudXYZI_PointXYZI::InitAsDefaultInstance() { -} - -PointCloudXYZI_PointXYZI::PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void PointCloudXYZI_PointXYZI::SharedCtor() { - _cached_size_ = 0; - x_ = 0; - y_ = 0; - z_ = 0; - intensity_ = 0; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -PointCloudXYZI_PointXYZI::~PointCloudXYZI_PointXYZI() { - SharedDtor(); -} - -void PointCloudXYZI_PointXYZI::SharedDtor() { - if (this != default_instance_) { - } -} - -void PointCloudXYZI_PointXYZI::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* PointCloudXYZI_PointXYZI::descriptor() { - protobuf_AssignDescriptorsOnce(); - return PointCloudXYZI_PointXYZI_descriptor_; -} - -const PointCloudXYZI_PointXYZI& PointCloudXYZI_PointXYZI::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -PointCloudXYZI_PointXYZI* PointCloudXYZI_PointXYZI::default_instance_ = NULL; - -PointCloudXYZI_PointXYZI* PointCloudXYZI_PointXYZI::New() const { - return new PointCloudXYZI_PointXYZI; -} - -void PointCloudXYZI_PointXYZI::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - x_ = 0; - y_ = 0; - z_ = 0; - intensity_ = 0; - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool PointCloudXYZI_PointXYZI::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required float x = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &x_))); - set_has_x(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(21)) goto parse_y; - break; - } - - // required float y = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_y: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &y_))); - set_has_y(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(29)) goto parse_z; - break; - } - - // required float z = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_z: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &z_))); - set_has_z(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(37)) goto parse_intensity; - break; - } - - // required float intensity = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_intensity: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &intensity_))); - set_has_intensity(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void PointCloudXYZI_PointXYZI::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required float x = 1; - if (has_x()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output); - } - - // required float y = 2; - if (has_y()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output); - } - - // required float z = 3; - if (has_z()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output); - } - - // required float intensity = 4; - if (has_intensity()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->intensity(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* PointCloudXYZI_PointXYZI::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required float x = 1; - if (has_x()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target); - } - - // required float y = 2; - if (has_y()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target); - } - - // required float z = 3; - if (has_z()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target); - } - - // required float intensity = 4; - if (has_intensity()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->intensity(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int PointCloudXYZI_PointXYZI::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required float x = 1; - if (has_x()) { - total_size += 1 + 4; - } - - // required float y = 2; - if (has_y()) { - total_size += 1 + 4; - } - - // required float z = 3; - if (has_z()) { - total_size += 1 + 4; - } - - // required float intensity = 4; - if (has_intensity()) { - total_size += 1 + 4; - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void PointCloudXYZI_PointXYZI::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const PointCloudXYZI_PointXYZI* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void PointCloudXYZI_PointXYZI::MergeFrom(const PointCloudXYZI_PointXYZI& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_x()) { - set_x(from.x()); - } - if (from.has_y()) { - set_y(from.y()); - } - if (from.has_z()) { - set_z(from.z()); - } - if (from.has_intensity()) { - set_intensity(from.intensity()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void PointCloudXYZI_PointXYZI::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void PointCloudXYZI_PointXYZI::CopyFrom(const PointCloudXYZI_PointXYZI& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool PointCloudXYZI_PointXYZI::IsInitialized() const { - if ((_has_bits_[0] & 0x0000000f) != 0x0000000f) return false; - - return true; -} - -void PointCloudXYZI_PointXYZI::Swap(PointCloudXYZI_PointXYZI* other) { - if (other != this) { - std::swap(x_, other->x_); - std::swap(y_, other->y_); - std::swap(z_, other->z_); - std::swap(intensity_, other->intensity_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata PointCloudXYZI_PointXYZI::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = PointCloudXYZI_PointXYZI_descriptor_; - metadata.reflection = PointCloudXYZI_PointXYZI_reflection_; - return metadata; -} - - -// ------------------------------------------------------------------- - -#ifndef _MSC_VER -const int PointCloudXYZI::kHeaderFieldNumber; -const int PointCloudXYZI::kPointsFieldNumber; -#endif // !_MSC_VER - -PointCloudXYZI::PointCloudXYZI() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void PointCloudXYZI::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -PointCloudXYZI::PointCloudXYZI(const PointCloudXYZI& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void PointCloudXYZI::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -PointCloudXYZI::~PointCloudXYZI() { - SharedDtor(); -} - -void PointCloudXYZI::SharedDtor() { - if (this != default_instance_) { - delete header_; - } -} - -void PointCloudXYZI::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* PointCloudXYZI::descriptor() { - protobuf_AssignDescriptorsOnce(); - return PointCloudXYZI_descriptor_; -} - -const PointCloudXYZI& PointCloudXYZI::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -PointCloudXYZI* PointCloudXYZI::default_instance_ = NULL; - -PointCloudXYZI* PointCloudXYZI::New() const { - return new PointCloudXYZI; -} - -void PointCloudXYZI::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - } - points_.Clear(); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool PointCloudXYZI::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_points; - break; - } - - // repeated .px.PointCloudXYZI.PointXYZI points = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_points: - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, add_points())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_points; - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void PointCloudXYZI::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // repeated .px.PointCloudXYZI.PointXYZI points = 2; - for (int i = 0; i < this->points_size(); i++) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 2, this->points(i), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* PointCloudXYZI::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // repeated .px.PointCloudXYZI.PointXYZI points = 2; - for (int i = 0; i < this->points_size(); i++) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 2, this->points(i), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int PointCloudXYZI::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - } - // repeated .px.PointCloudXYZI.PointXYZI points = 2; - total_size += 1 * this->points_size(); - for (int i = 0; i < this->points_size(); i++) { - total_size += - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->points(i)); - } - - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void PointCloudXYZI::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const PointCloudXYZI* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void PointCloudXYZI::MergeFrom(const PointCloudXYZI& from) { - GOOGLE_CHECK_NE(&from, this); - points_.MergeFrom(from.points_); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void PointCloudXYZI::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void PointCloudXYZI::CopyFrom(const PointCloudXYZI& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool PointCloudXYZI::IsInitialized() const { - if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - for (int i = 0; i < points_size(); i++) { - if (!this->points(i).IsInitialized()) return false; - } - return true; -} - -void PointCloudXYZI::Swap(PointCloudXYZI* other) { - if (other != this) { - std::swap(header_, other->header_); - points_.Swap(&other->points_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata PointCloudXYZI::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = PointCloudXYZI_descriptor_; - metadata.reflection = PointCloudXYZI_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int PointCloudXYZRGB_PointXYZRGB::kXFieldNumber; -const int PointCloudXYZRGB_PointXYZRGB::kYFieldNumber; -const int PointCloudXYZRGB_PointXYZRGB::kZFieldNumber; -const int PointCloudXYZRGB_PointXYZRGB::kRgbFieldNumber; -#endif // !_MSC_VER - -PointCloudXYZRGB_PointXYZRGB::PointCloudXYZRGB_PointXYZRGB() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void PointCloudXYZRGB_PointXYZRGB::InitAsDefaultInstance() { -} - -PointCloudXYZRGB_PointXYZRGB::PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void PointCloudXYZRGB_PointXYZRGB::SharedCtor() { - _cached_size_ = 0; - x_ = 0; - y_ = 0; - z_ = 0; - rgb_ = 0; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -PointCloudXYZRGB_PointXYZRGB::~PointCloudXYZRGB_PointXYZRGB() { - SharedDtor(); -} - -void PointCloudXYZRGB_PointXYZRGB::SharedDtor() { - if (this != default_instance_) { - } -} - -void PointCloudXYZRGB_PointXYZRGB::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* PointCloudXYZRGB_PointXYZRGB::descriptor() { - protobuf_AssignDescriptorsOnce(); - return PointCloudXYZRGB_PointXYZRGB_descriptor_; -} - -const PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB_PointXYZRGB::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB_PointXYZRGB::default_instance_ = NULL; - -PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB_PointXYZRGB::New() const { - return new PointCloudXYZRGB_PointXYZRGB; -} - -void PointCloudXYZRGB_PointXYZRGB::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - x_ = 0; - y_ = 0; - z_ = 0; - rgb_ = 0; - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool PointCloudXYZRGB_PointXYZRGB::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required float x = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &x_))); - set_has_x(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(21)) goto parse_y; - break; - } - - // required float y = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_y: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &y_))); - set_has_y(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(29)) goto parse_z; - break; - } - - // required float z = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_z: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &z_))); - set_has_z(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(37)) goto parse_rgb; - break; - } - - // required float rgb = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_rgb: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &rgb_))); - set_has_rgb(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required float x = 1; - if (has_x()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output); - } - - // required float y = 2; - if (has_y()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output); - } - - // required float z = 3; - if (has_z()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output); - } - - // required float rgb = 4; - if (has_rgb()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->rgb(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required float x = 1; - if (has_x()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target); - } - - // required float y = 2; - if (has_y()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target); - } - - // required float z = 3; - if (has_z()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target); - } - - // required float rgb = 4; - if (has_rgb()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->rgb(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int PointCloudXYZRGB_PointXYZRGB::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required float x = 1; - if (has_x()) { - total_size += 1 + 4; - } - - // required float y = 2; - if (has_y()) { - total_size += 1 + 4; - } - - // required float z = 3; - if (has_z()) { - total_size += 1 + 4; - } - - // required float rgb = 4; - if (has_rgb()) { - total_size += 1 + 4; - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const PointCloudXYZRGB_PointXYZRGB* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const PointCloudXYZRGB_PointXYZRGB& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_x()) { - set_x(from.x()); - } - if (from.has_y()) { - set_y(from.y()); - } - if (from.has_z()) { - set_z(from.z()); - } - if (from.has_rgb()) { - set_rgb(from.rgb()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void PointCloudXYZRGB_PointXYZRGB::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void PointCloudXYZRGB_PointXYZRGB::CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool PointCloudXYZRGB_PointXYZRGB::IsInitialized() const { - if ((_has_bits_[0] & 0x0000000f) != 0x0000000f) return false; - - return true; -} - -void PointCloudXYZRGB_PointXYZRGB::Swap(PointCloudXYZRGB_PointXYZRGB* other) { - if (other != this) { - std::swap(x_, other->x_); - std::swap(y_, other->y_); - std::swap(z_, other->z_); - std::swap(rgb_, other->rgb_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata PointCloudXYZRGB_PointXYZRGB::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = PointCloudXYZRGB_PointXYZRGB_descriptor_; - metadata.reflection = PointCloudXYZRGB_PointXYZRGB_reflection_; - return metadata; -} - - -// ------------------------------------------------------------------- - -#ifndef _MSC_VER -const int PointCloudXYZRGB::kHeaderFieldNumber; -const int PointCloudXYZRGB::kPointsFieldNumber; -#endif // !_MSC_VER - -PointCloudXYZRGB::PointCloudXYZRGB() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void PointCloudXYZRGB::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -PointCloudXYZRGB::PointCloudXYZRGB(const PointCloudXYZRGB& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void PointCloudXYZRGB::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -PointCloudXYZRGB::~PointCloudXYZRGB() { - SharedDtor(); -} - -void PointCloudXYZRGB::SharedDtor() { - if (this != default_instance_) { - delete header_; - } -} - -void PointCloudXYZRGB::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* PointCloudXYZRGB::descriptor() { - protobuf_AssignDescriptorsOnce(); - return PointCloudXYZRGB_descriptor_; -} - -const PointCloudXYZRGB& PointCloudXYZRGB::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -PointCloudXYZRGB* PointCloudXYZRGB::default_instance_ = NULL; - -PointCloudXYZRGB* PointCloudXYZRGB::New() const { - return new PointCloudXYZRGB; -} - -void PointCloudXYZRGB::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - } - points_.Clear(); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool PointCloudXYZRGB::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_points; - break; - } - - // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_points: - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, add_points())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_points; - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void PointCloudXYZRGB::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; - for (int i = 0; i < this->points_size(); i++) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 2, this->points(i), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* PointCloudXYZRGB::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; - for (int i = 0; i < this->points_size(); i++) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 2, this->points(i), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int PointCloudXYZRGB::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - } - // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; - total_size += 1 * this->points_size(); - for (int i = 0; i < this->points_size(); i++) { - total_size += - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->points(i)); - } - - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void PointCloudXYZRGB::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const PointCloudXYZRGB* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void PointCloudXYZRGB::MergeFrom(const PointCloudXYZRGB& from) { - GOOGLE_CHECK_NE(&from, this); - points_.MergeFrom(from.points_); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void PointCloudXYZRGB::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void PointCloudXYZRGB::CopyFrom(const PointCloudXYZRGB& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool PointCloudXYZRGB::IsInitialized() const { - if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - for (int i = 0; i < points_size(); i++) { - if (!this->points(i).IsInitialized()) return false; - } - return true; -} - -void PointCloudXYZRGB::Swap(PointCloudXYZRGB* other) { - if (other != this) { - std::swap(header_, other->header_); - points_.Swap(&other->points_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata PointCloudXYZRGB::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = PointCloudXYZRGB_descriptor_; - metadata.reflection = PointCloudXYZRGB_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int RGBDImage::kHeaderFieldNumber; -const int RGBDImage::kColsFieldNumber; -const int RGBDImage::kRowsFieldNumber; -const int RGBDImage::kStep1FieldNumber; -const int RGBDImage::kType1FieldNumber; -const int RGBDImage::kImageData1FieldNumber; -const int RGBDImage::kStep2FieldNumber; -const int RGBDImage::kType2FieldNumber; -const int RGBDImage::kImageData2FieldNumber; -const int RGBDImage::kCameraConfigFieldNumber; -const int RGBDImage::kCameraTypeFieldNumber; -const int RGBDImage::kRollFieldNumber; -const int RGBDImage::kPitchFieldNumber; -const int RGBDImage::kYawFieldNumber; -const int RGBDImage::kLonFieldNumber; -const int RGBDImage::kLatFieldNumber; -const int RGBDImage::kAltFieldNumber; -const int RGBDImage::kGroundXFieldNumber; -const int RGBDImage::kGroundYFieldNumber; -const int RGBDImage::kGroundZFieldNumber; -const int RGBDImage::kCameraMatrixFieldNumber; -#endif // !_MSC_VER - -RGBDImage::RGBDImage() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void RGBDImage::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -RGBDImage::RGBDImage(const RGBDImage& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void RGBDImage::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - cols_ = 0u; - rows_ = 0u; - step1_ = 0u; - type1_ = 0u; - imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - step2_ = 0u; - type2_ = 0u; - imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - camera_config_ = 0u; - camera_type_ = 0u; - roll_ = 0; - pitch_ = 0; - yaw_ = 0; - lon_ = 0; - lat_ = 0; - alt_ = 0; - ground_x_ = 0; - ground_y_ = 0; - ground_z_ = 0; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -RGBDImage::~RGBDImage() { - SharedDtor(); -} - -void RGBDImage::SharedDtor() { - if (imagedata1_ != &::google::protobuf::internal::kEmptyString) { - delete imagedata1_; - } - if (imagedata2_ != &::google::protobuf::internal::kEmptyString) { - delete imagedata2_; - } - if (this != default_instance_) { - delete header_; - } -} - -void RGBDImage::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* RGBDImage::descriptor() { - protobuf_AssignDescriptorsOnce(); - return RGBDImage_descriptor_; -} - -const RGBDImage& RGBDImage::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -RGBDImage* RGBDImage::default_instance_ = NULL; - -RGBDImage* RGBDImage::New() const { - return new RGBDImage; -} - -void RGBDImage::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - cols_ = 0u; - rows_ = 0u; - step1_ = 0u; - type1_ = 0u; - if (has_imagedata1()) { - if (imagedata1_ != &::google::protobuf::internal::kEmptyString) { - imagedata1_->clear(); - } - } - step2_ = 0u; - type2_ = 0u; - } - if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) { - if (has_imagedata2()) { - if (imagedata2_ != &::google::protobuf::internal::kEmptyString) { - imagedata2_->clear(); - } - } - camera_config_ = 0u; - camera_type_ = 0u; - roll_ = 0; - pitch_ = 0; - yaw_ = 0; - lon_ = 0; - lat_ = 0; - } - if (_has_bits_[16 / 32] & (0xffu << (16 % 32))) { - alt_ = 0; - ground_x_ = 0; - ground_y_ = 0; - ground_z_ = 0; - } - camera_matrix_.Clear(); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool RGBDImage::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(16)) goto parse_cols; - break; - } - - // required uint32 cols = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_cols: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &cols_))); - set_has_cols(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(24)) goto parse_rows; - break; - } - - // required uint32 rows = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_rows: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &rows_))); - set_has_rows(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(32)) goto parse_step1; - break; - } - - // required uint32 step1 = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_step1: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &step1_))); - set_has_step1(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(40)) goto parse_type1; - break; - } - - // required uint32 type1 = 5; - case 5: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_type1: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &type1_))); - set_has_type1(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(50)) goto parse_imageData1; - break; - } - - // required bytes imageData1 = 6; - case 6: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_imageData1: - DO_(::google::protobuf::internal::WireFormatLite::ReadBytes( - input, this->mutable_imagedata1())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(56)) goto parse_step2; - break; - } - - // required uint32 step2 = 7; - case 7: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_step2: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &step2_))); - set_has_step2(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(64)) goto parse_type2; - break; - } - - // required uint32 type2 = 8; - case 8: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_type2: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &type2_))); - set_has_type2(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(74)) goto parse_imageData2; - break; - } - - // required bytes imageData2 = 9; - case 9: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_imageData2: - DO_(::google::protobuf::internal::WireFormatLite::ReadBytes( - input, this->mutable_imagedata2())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(80)) goto parse_camera_config; - break; - } - - // optional uint32 camera_config = 10; - case 10: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_camera_config: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &camera_config_))); - set_has_camera_config(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(88)) goto parse_camera_type; - break; - } - - // optional uint32 camera_type = 11; - case 11: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_camera_type: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &camera_type_))); - set_has_camera_type(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(101)) goto parse_roll; - break; - } - - // optional float roll = 12; - case 12: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_roll: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &roll_))); - set_has_roll(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(109)) goto parse_pitch; - break; - } - - // optional float pitch = 13; - case 13: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_pitch: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &pitch_))); - set_has_pitch(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(117)) goto parse_yaw; - break; - } - - // optional float yaw = 14; - case 14: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_yaw: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &yaw_))); - set_has_yaw(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(125)) goto parse_lon; - break; - } - - // optional float lon = 15; - case 15: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_lon: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &lon_))); - set_has_lon(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(133)) goto parse_lat; - break; - } - - // optional float lat = 16; - case 16: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_lat: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &lat_))); - set_has_lat(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(141)) goto parse_alt; - break; - } - - // optional float alt = 17; - case 17: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_alt: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &alt_))); - set_has_alt(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(149)) goto parse_ground_x; - break; - } - - // optional float ground_x = 18; - case 18: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_ground_x: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &ground_x_))); - set_has_ground_x(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(157)) goto parse_ground_y; - break; - } - - // optional float ground_y = 19; - case 19: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_ground_y: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &ground_y_))); - set_has_ground_y(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(165)) goto parse_ground_z; - break; - } - - // optional float ground_z = 20; - case 20: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_ground_z: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &ground_z_))); - set_has_ground_z(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(173)) goto parse_camera_matrix; - break; - } - - // repeated float camera_matrix = 21; - case 21: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_camera_matrix: - DO_((::google::protobuf::internal::WireFormatLite::ReadRepeatedPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - 2, 173, input, this->mutable_camera_matrix()))); - } else if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) - == ::google::protobuf::internal::WireFormatLite:: - WIRETYPE_LENGTH_DELIMITED) { - DO_((::google::protobuf::internal::WireFormatLite::ReadPackedPrimitiveNoInline< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, this->mutable_camera_matrix()))); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(173)) goto parse_camera_matrix; - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void RGBDImage::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // required uint32 cols = 2; - if (has_cols()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(2, this->cols(), output); - } - - // required uint32 rows = 3; - if (has_rows()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(3, this->rows(), output); - } - - // required uint32 step1 = 4; - if (has_step1()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(4, this->step1(), output); - } - - // required uint32 type1 = 5; - if (has_type1()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(5, this->type1(), output); - } - - // required bytes imageData1 = 6; - if (has_imagedata1()) { - ::google::protobuf::internal::WireFormatLite::WriteBytes( - 6, this->imagedata1(), output); - } - - // required uint32 step2 = 7; - if (has_step2()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(7, this->step2(), output); - } - - // required uint32 type2 = 8; - if (has_type2()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(8, this->type2(), output); - } - - // required bytes imageData2 = 9; - if (has_imagedata2()) { - ::google::protobuf::internal::WireFormatLite::WriteBytes( - 9, this->imagedata2(), output); - } - - // optional uint32 camera_config = 10; - if (has_camera_config()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(10, this->camera_config(), output); - } - - // optional uint32 camera_type = 11; - if (has_camera_type()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(11, this->camera_type(), output); - } - - // optional float roll = 12; - if (has_roll()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(12, this->roll(), output); - } - - // optional float pitch = 13; - if (has_pitch()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(13, this->pitch(), output); - } - - // optional float yaw = 14; - if (has_yaw()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(14, this->yaw(), output); - } - - // optional float lon = 15; - if (has_lon()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(15, this->lon(), output); - } - - // optional float lat = 16; - if (has_lat()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(16, this->lat(), output); - } - - // optional float alt = 17; - if (has_alt()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(17, this->alt(), output); - } - - // optional float ground_x = 18; - if (has_ground_x()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(18, this->ground_x(), output); - } - - // optional float ground_y = 19; - if (has_ground_y()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(19, this->ground_y(), output); - } - - // optional float ground_z = 20; - if (has_ground_z()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(20, this->ground_z(), output); - } - - // repeated float camera_matrix = 21; - for (int i = 0; i < this->camera_matrix_size(); i++) { - ::google::protobuf::internal::WireFormatLite::WriteFloat( - 21, this->camera_matrix(i), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* RGBDImage::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // required uint32 cols = 2; - if (has_cols()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(2, this->cols(), target); - } - - // required uint32 rows = 3; - if (has_rows()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(3, this->rows(), target); - } - - // required uint32 step1 = 4; - if (has_step1()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(4, this->step1(), target); - } - - // required uint32 type1 = 5; - if (has_type1()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(5, this->type1(), target); - } - - // required bytes imageData1 = 6; - if (has_imagedata1()) { - target = - ::google::protobuf::internal::WireFormatLite::WriteBytesToArray( - 6, this->imagedata1(), target); - } - - // required uint32 step2 = 7; - if (has_step2()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(7, this->step2(), target); - } - - // required uint32 type2 = 8; - if (has_type2()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(8, this->type2(), target); - } - - // required bytes imageData2 = 9; - if (has_imagedata2()) { - target = - ::google::protobuf::internal::WireFormatLite::WriteBytesToArray( - 9, this->imagedata2(), target); - } - - // optional uint32 camera_config = 10; - if (has_camera_config()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(10, this->camera_config(), target); - } - - // optional uint32 camera_type = 11; - if (has_camera_type()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(11, this->camera_type(), target); - } - - // optional float roll = 12; - if (has_roll()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(12, this->roll(), target); - } - - // optional float pitch = 13; - if (has_pitch()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(13, this->pitch(), target); - } - - // optional float yaw = 14; - if (has_yaw()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(14, this->yaw(), target); - } - - // optional float lon = 15; - if (has_lon()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(15, this->lon(), target); - } - - // optional float lat = 16; - if (has_lat()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(16, this->lat(), target); - } - - // optional float alt = 17; - if (has_alt()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(17, this->alt(), target); - } - - // optional float ground_x = 18; - if (has_ground_x()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(18, this->ground_x(), target); - } - - // optional float ground_y = 19; - if (has_ground_y()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(19, this->ground_y(), target); - } - - // optional float ground_z = 20; - if (has_ground_z()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(20, this->ground_z(), target); - } - - // repeated float camera_matrix = 21; - for (int i = 0; i < this->camera_matrix_size(); i++) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteFloatToArray(21, this->camera_matrix(i), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int RGBDImage::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - // required uint32 cols = 2; - if (has_cols()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->cols()); - } - - // required uint32 rows = 3; - if (has_rows()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->rows()); - } - - // required uint32 step1 = 4; - if (has_step1()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->step1()); - } - - // required uint32 type1 = 5; - if (has_type1()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->type1()); - } - - // required bytes imageData1 = 6; - if (has_imagedata1()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::BytesSize( - this->imagedata1()); - } - - // required uint32 step2 = 7; - if (has_step2()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->step2()); - } - - // required uint32 type2 = 8; - if (has_type2()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->type2()); - } - - } - if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) { - // required bytes imageData2 = 9; - if (has_imagedata2()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::BytesSize( - this->imagedata2()); - } - - // optional uint32 camera_config = 10; - if (has_camera_config()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->camera_config()); - } - - // optional uint32 camera_type = 11; - if (has_camera_type()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->camera_type()); - } - - // optional float roll = 12; - if (has_roll()) { - total_size += 1 + 4; - } - - // optional float pitch = 13; - if (has_pitch()) { - total_size += 1 + 4; - } - - // optional float yaw = 14; - if (has_yaw()) { - total_size += 1 + 4; - } - - // optional float lon = 15; - if (has_lon()) { - total_size += 1 + 4; - } - - // optional float lat = 16; - if (has_lat()) { - total_size += 2 + 4; - } - - } - if (_has_bits_[16 / 32] & (0xffu << (16 % 32))) { - // optional float alt = 17; - if (has_alt()) { - total_size += 2 + 4; - } - - // optional float ground_x = 18; - if (has_ground_x()) { - total_size += 2 + 4; - } - - // optional float ground_y = 19; - if (has_ground_y()) { - total_size += 2 + 4; - } - - // optional float ground_z = 20; - if (has_ground_z()) { - total_size += 2 + 4; - } - - } - // repeated float camera_matrix = 21; - { - int data_size = 0; - data_size = 4 * this->camera_matrix_size(); - total_size += 2 * this->camera_matrix_size() + data_size; - } - - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void RGBDImage::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const RGBDImage* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void RGBDImage::MergeFrom(const RGBDImage& from) { - GOOGLE_CHECK_NE(&from, this); - camera_matrix_.MergeFrom(from.camera_matrix_); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - if (from.has_cols()) { - set_cols(from.cols()); - } - if (from.has_rows()) { - set_rows(from.rows()); - } - if (from.has_step1()) { - set_step1(from.step1()); - } - if (from.has_type1()) { - set_type1(from.type1()); - } - if (from.has_imagedata1()) { - set_imagedata1(from.imagedata1()); - } - if (from.has_step2()) { - set_step2(from.step2()); - } - if (from.has_type2()) { - set_type2(from.type2()); - } - } - if (from._has_bits_[8 / 32] & (0xffu << (8 % 32))) { - if (from.has_imagedata2()) { - set_imagedata2(from.imagedata2()); - } - if (from.has_camera_config()) { - set_camera_config(from.camera_config()); - } - if (from.has_camera_type()) { - set_camera_type(from.camera_type()); - } - if (from.has_roll()) { - set_roll(from.roll()); - } - if (from.has_pitch()) { - set_pitch(from.pitch()); - } - if (from.has_yaw()) { - set_yaw(from.yaw()); - } - if (from.has_lon()) { - set_lon(from.lon()); - } - if (from.has_lat()) { - set_lat(from.lat()); - } - } - if (from._has_bits_[16 / 32] & (0xffu << (16 % 32))) { - if (from.has_alt()) { - set_alt(from.alt()); - } - if (from.has_ground_x()) { - set_ground_x(from.ground_x()); - } - if (from.has_ground_y()) { - set_ground_y(from.ground_y()); - } - if (from.has_ground_z()) { - set_ground_z(from.ground_z()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void RGBDImage::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void RGBDImage::CopyFrom(const RGBDImage& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool RGBDImage::IsInitialized() const { - if ((_has_bits_[0] & 0x000001ff) != 0x000001ff) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - return true; -} - -void RGBDImage::Swap(RGBDImage* other) { - if (other != this) { - std::swap(header_, other->header_); - std::swap(cols_, other->cols_); - std::swap(rows_, other->rows_); - std::swap(step1_, other->step1_); - std::swap(type1_, other->type1_); - std::swap(imagedata1_, other->imagedata1_); - std::swap(step2_, other->step2_); - std::swap(type2_, other->type2_); - std::swap(imagedata2_, other->imagedata2_); - std::swap(camera_config_, other->camera_config_); - std::swap(camera_type_, other->camera_type_); - std::swap(roll_, other->roll_); - std::swap(pitch_, other->pitch_); - std::swap(yaw_, other->yaw_); - std::swap(lon_, other->lon_); - std::swap(lat_, other->lat_); - std::swap(alt_, other->alt_); - std::swap(ground_x_, other->ground_x_); - std::swap(ground_y_, other->ground_y_); - std::swap(ground_z_, other->ground_z_); - camera_matrix_.Swap(&other->camera_matrix_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata RGBDImage::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = RGBDImage_descriptor_; - metadata.reflection = RGBDImage_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int Waypoint::kXFieldNumber; -const int Waypoint::kYFieldNumber; -const int Waypoint::kZFieldNumber; -const int Waypoint::kRollFieldNumber; -const int Waypoint::kPitchFieldNumber; -const int Waypoint::kYawFieldNumber; -#endif // !_MSC_VER - -Waypoint::Waypoint() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void Waypoint::InitAsDefaultInstance() { -} - -Waypoint::Waypoint(const Waypoint& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void Waypoint::SharedCtor() { - _cached_size_ = 0; - x_ = 0; - y_ = 0; - z_ = 0; - roll_ = 0; - pitch_ = 0; - yaw_ = 0; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -Waypoint::~Waypoint() { - SharedDtor(); -} - -void Waypoint::SharedDtor() { - if (this != default_instance_) { - } -} - -void Waypoint::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* Waypoint::descriptor() { - protobuf_AssignDescriptorsOnce(); - return Waypoint_descriptor_; -} - -const Waypoint& Waypoint::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -Waypoint* Waypoint::default_instance_ = NULL; - -Waypoint* Waypoint::New() const { - return new Waypoint; -} - -void Waypoint::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - x_ = 0; - y_ = 0; - z_ = 0; - roll_ = 0; - pitch_ = 0; - yaw_ = 0; - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool Waypoint::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required double x = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &x_))); - set_has_x(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(17)) goto parse_y; - break; - } - - // required double y = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_y: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &y_))); - set_has_y(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(25)) goto parse_z; - break; - } - - // optional double z = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_z: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &z_))); - set_has_z(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(33)) goto parse_roll; - break; - } - - // optional double roll = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_roll: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &roll_))); - set_has_roll(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(41)) goto parse_pitch; - break; - } - - // optional double pitch = 5; - case 5: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_pitch: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &pitch_))); - set_has_pitch(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(49)) goto parse_yaw; - break; - } - - // optional double yaw = 6; - case 6: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_yaw: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &yaw_))); - set_has_yaw(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void Waypoint::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required double x = 1; - if (has_x()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(1, this->x(), output); - } - - // required double y = 2; - if (has_y()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(2, this->y(), output); - } - - // optional double z = 3; - if (has_z()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(3, this->z(), output); - } - - // optional double roll = 4; - if (has_roll()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(4, this->roll(), output); - } - - // optional double pitch = 5; - if (has_pitch()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(5, this->pitch(), output); - } - - // optional double yaw = 6; - if (has_yaw()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(6, this->yaw(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* Waypoint::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required double x = 1; - if (has_x()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(1, this->x(), target); - } - - // required double y = 2; - if (has_y()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(2, this->y(), target); - } - - // optional double z = 3; - if (has_z()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(3, this->z(), target); - } - - // optional double roll = 4; - if (has_roll()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(4, this->roll(), target); - } - - // optional double pitch = 5; - if (has_pitch()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(5, this->pitch(), target); - } - - // optional double yaw = 6; - if (has_yaw()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(6, this->yaw(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int Waypoint::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required double x = 1; - if (has_x()) { - total_size += 1 + 8; - } - - // required double y = 2; - if (has_y()) { - total_size += 1 + 8; - } - - // optional double z = 3; - if (has_z()) { - total_size += 1 + 8; - } - - // optional double roll = 4; - if (has_roll()) { - total_size += 1 + 8; - } - - // optional double pitch = 5; - if (has_pitch()) { - total_size += 1 + 8; - } - - // optional double yaw = 6; - if (has_yaw()) { - total_size += 1 + 8; - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void Waypoint::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const Waypoint* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void Waypoint::MergeFrom(const Waypoint& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_x()) { - set_x(from.x()); - } - if (from.has_y()) { - set_y(from.y()); - } - if (from.has_z()) { - set_z(from.z()); - } - if (from.has_roll()) { - set_roll(from.roll()); - } - if (from.has_pitch()) { - set_pitch(from.pitch()); - } - if (from.has_yaw()) { - set_yaw(from.yaw()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void Waypoint::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void Waypoint::CopyFrom(const Waypoint& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool Waypoint::IsInitialized() const { - if ((_has_bits_[0] & 0x00000003) != 0x00000003) return false; - - return true; -} - -void Waypoint::Swap(Waypoint* other) { - if (other != this) { - std::swap(x_, other->x_); - std::swap(y_, other->y_); - std::swap(z_, other->z_); - std::swap(roll_, other->roll_); - std::swap(pitch_, other->pitch_); - std::swap(yaw_, other->yaw_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata Waypoint::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = Waypoint_descriptor_; - metadata.reflection = Waypoint_reflection_; - return metadata; -} - - -// @@protoc_insertion_point(namespace_scope) - -} // namespace px - -// @@protoc_insertion_point(global_scope) diff --git a/mavlink/share/pyshared/pymavlink/.gitignore b/mavlink/share/pyshared/pymavlink/.gitignore deleted file mode 100644 index 3d395ecda..000000000 --- a/mavlink/share/pyshared/pymavlink/.gitignore +++ /dev/null @@ -1,13 +0,0 @@ -apidocs/ -*.zip -*.pyc -send.sh -generator/C/include/ardupilotmega -generator/C/include/common -generator/C/include/pixhawk -generator/C/include/minimal -generator/C/include/ualberta -generator/C/include/slugs -testmav0.9* -testmav1.0* -Debug/ diff --git a/mavlink/share/pyshared/pymavlink/APM_Mavtest/APM_Mavtest.pde b/mavlink/share/pyshared/pymavlink/APM_Mavtest/APM_Mavtest.pde deleted file mode 100644 index b903b5c16..000000000 --- a/mavlink/share/pyshared/pymavlink/APM_Mavtest/APM_Mavtest.pde +++ /dev/null @@ -1,55 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/* - send all possible mavlink messages - Andrew Tridgell July 2011 -*/ - -// AVR runtime -#include -#include -#include -#include - -// Libraries -#include -#include -#include -#include "mavtest.h" - -FastSerialPort0(Serial); // FTDI/console -FastSerialPort1(Serial1); // GPS port -FastSerialPort3(Serial3); // Telemetry port - -#define SERIAL0_BAUD 115200 -#define SERIAL3_BAUD 115200 - -void setup() { - Serial.begin(SERIAL0_BAUD, 128, 128); - Serial3.begin(SERIAL3_BAUD, 128, 128); - mavlink_comm_0_port = &Serial; - mavlink_comm_1_port = &Serial3; -} - - - -void loop() -{ - Serial.println("Starting MAVLink test generator\n"); - while (1) { - mavlink_msg_heartbeat_send( - MAVLINK_COMM_0, - mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); - - mavlink_msg_heartbeat_send( - MAVLINK_COMM_1, - mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); - - mavtest_generate_outputs(MAVLINK_COMM_0); - mavtest_generate_outputs(MAVLINK_COMM_1); - delay(500); - } -} - diff --git a/mavlink/share/pyshared/pymavlink/README.txt b/mavlink/share/pyshared/pymavlink/README.txt deleted file mode 100644 index 386013e11..000000000 --- a/mavlink/share/pyshared/pymavlink/README.txt +++ /dev/null @@ -1,4 +0,0 @@ -This is a python implementation of the MAVLink protocol. - -Please see http://www.qgroundcontrol.org/mavlink/pymavlink for -documentation diff --git a/mavlink/share/pyshared/pymavlink/examples/apmsetrate.py b/mavlink/share/pyshared/pymavlink/examples/apmsetrate.py deleted file mode 100644 index d7d82256c..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/apmsetrate.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python - -''' -set stream rate on an APM -''' - -import sys, struct, time, os - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -from optparse import OptionParser -parser = OptionParser("apmsetrate.py [options]") - -parser.add_option("--baudrate", dest="baudrate", type='int', - help="master port baud rate", default=115200) -parser.add_option("--device", dest="device", default=None, help="serial device") -parser.add_option("--rate", dest="rate", default=4, type='int', help="requested stream rate") -parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int', - default=255, help='MAVLink source system for this GCS') -parser.add_option("--showmessages", dest="showmessages", action='store_true', - help="show incoming messages", default=False) -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' - import mavlink10 as mavlink -else: - import mavlink -import mavutil - -if opts.device is None: - print("You must specify a serial device") - sys.exit(1) - -def wait_heartbeat(m): - '''wait for a heartbeat so we know the target system IDs''' - print("Waiting for APM heartbeat") - m.wait_heartbeat() - print("Heartbeat from APM (system %u component %u)" % (m.target_system, m.target_system)) - -def show_messages(m): - '''show incoming mavlink messages''' - while True: - msg = m.recv_match(blocking=True) - if not msg: - return - if msg.get_type() == "BAD_DATA": - if mavutil.all_printable(msg.data): - sys.stdout.write(msg.data) - sys.stdout.flush() - else: - print(msg) - -# create a mavlink serial instance -master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate) - -# wait for the heartbeat msg to find the system ID -wait_heartbeat(master) - -print("Sending all stream request for rate %u" % opts.rate) -for i in range(0, 3): - master.mav.request_data_stream_send(master.target_system, master.target_component, - mavlink.MAV_DATA_STREAM_ALL, opts.rate, 1) -if opts.showmessages: - show_messages(master) diff --git a/mavlink/share/pyshared/pymavlink/examples/bwtest.py b/mavlink/share/pyshared/pymavlink/examples/bwtest.py deleted file mode 100644 index de56d4f8c..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/bwtest.py +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/env python - -''' -check bandwidth of link -''' - -import sys, struct, time, os - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -import mavutil - -from optparse import OptionParser -parser = OptionParser("bwtest.py [options]") - -parser.add_option("--baudrate", dest="baudrate", type='int', - help="master port baud rate", default=115200) -parser.add_option("--device", dest="device", default=None, help="serial device") -(opts, args) = parser.parse_args() - -if opts.device is None: - print("You must specify a serial device") - sys.exit(1) - -# create a mavlink serial instance -master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate) - -t1 = time.time() - -counts = {} - -bytes_sent = 0 -bytes_recv = 0 - -while True: - master.mav.heartbeat_send(1, 1) - master.mav.sys_status_send(1, 2, 3, 4, 5, 6, 7) - master.mav.gps_raw_send(1, 2, 3, 4, 5, 6, 7, 8, 9) - master.mav.attitude_send(1, 2, 3, 4, 5, 6, 7) - master.mav.vfr_hud_send(1, 2, 3, 4, 5, 6) - while master.port.inWaiting() > 0: - m = master.recv_msg() - if m == None: break - if m.get_type() not in counts: - counts[m.get_type()] = 0 - counts[m.get_type()] += 1 - t2 = time.time() - if t2 - t1 > 1.0: - print("%u sent, %u received, %u errors bwin=%.1f kB/s bwout=%.1f kB/s" % ( - master.mav.total_packets_sent, - master.mav.total_packets_received, - master.mav.total_receive_errors, - 0.001*(master.mav.total_bytes_received-bytes_recv)/(t2-t1), - 0.001*(master.mav.total_bytes_sent-bytes_sent)/(t2-t1))) - bytes_sent = master.mav.total_bytes_sent - bytes_recv = master.mav.total_bytes_received - t1 = t2 diff --git a/mavlink/share/pyshared/pymavlink/examples/flightmodes.py b/mavlink/share/pyshared/pymavlink/examples/flightmodes.py deleted file mode 100644 index 03d7e2c47..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/flightmodes.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python - -''' -show changes in flight modes -''' - -import sys, time, os - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -from optparse import OptionParser -parser = OptionParser("flightmodes.py [options]") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") - -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 1: - print("Usage: flightmodes.py [options] ") - sys.exit(1) - -def flight_modes(logfile): - '''show flight modes for a log file''' - print("Processing log %s" % filename) - mlog = mavutil.mavlink_connection(filename) - - mode = -1 - nav_mode = -1 - - filesize = os.path.getsize(filename) - - while True: - m = mlog.recv_match(type='SYS_STATUS', - condition='SYS_STATUS.mode != %u or SYS_STATUS.nav_mode != %u' % (mode, nav_mode)) - if m is None: - return - mode = m.mode - nav_mode = m.nav_mode - pct = (100.0 * mlog.f.tell()) / filesize - print('%s MAV.flightmode=%-12s mode=%u nav_mode=%u (MAV.timestamp=%u %u%%)' % ( - time.asctime(time.localtime(m._timestamp)), - mlog.flightmode, - mode, nav_mode, m._timestamp, pct)) - -for filename in args: - flight_modes(filename) - - diff --git a/mavlink/share/pyshared/pymavlink/examples/flighttime.py b/mavlink/share/pyshared/pymavlink/examples/flighttime.py deleted file mode 100644 index 81cd38c94..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/flighttime.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python - -''' -work out total flight time for a mavlink log -''' - -import sys, time, os - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -from optparse import OptionParser -parser = OptionParser("flighttime.py [options]") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") - -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 1: - print("Usage: flighttime.py [options] ") - sys.exit(1) - -def flight_time(logfile): - '''work out flight time for a log file''' - print("Processing log %s" % filename) - mlog = mavutil.mavlink_connection(filename) - - in_air = False - start_time = 0.0 - total_time = 0.0 - t = None - - while True: - m = mlog.recv_match(type='VFR_HUD') - if m is None: - if in_air: - total_time += time.mktime(t) - start_time - if total_time > 0: - print("Flight time : %u:%02u" % (int(total_time)/60, int(total_time)%60)) - return total_time - t = time.localtime(m._timestamp) - if m.groundspeed > 3.0 and not in_air: - print("In air at %s" % time.asctime(t)) - in_air = True - start_time = time.mktime(t) - elif m.groundspeed < 3.0 and in_air: - print("On ground at %s" % time.asctime(t)) - in_air = False - total_time += time.mktime(t) - start_time - return total_time - -total = 0.0 -for filename in args: - total += flight_time(filename) - -print("Total time in air: %u:%02u" % (int(total)/60, int(total)%60)) diff --git a/mavlink/share/pyshared/pymavlink/examples/gpslock.py b/mavlink/share/pyshared/pymavlink/examples/gpslock.py deleted file mode 100644 index f15b29072..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/gpslock.py +++ /dev/null @@ -1,68 +0,0 @@ -#!/usr/bin/env python - -''' -show GPS lock events in a MAVLink log -''' - -import sys, time, os - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -from optparse import OptionParser -parser = OptionParser("gpslock.py [options]") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") - -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 1: - print("Usage: gpslock.py [options] ") - sys.exit(1) - -def lock_time(logfile): - '''work out gps lock times for a log file''' - print("Processing log %s" % filename) - mlog = mavutil.mavlink_connection(filename) - - locked = False - start_time = 0.0 - total_time = 0.0 - t = None - m = mlog.recv_match(type='GPS_RAW') - unlock_time = time.mktime(time.localtime(m._timestamp)) - - while True: - m = mlog.recv_match(type='GPS_RAW') - if m is None: - if locked: - total_time += time.mktime(t) - start_time - if total_time > 0: - print("Lock time : %u:%02u" % (int(total_time)/60, int(total_time)%60)) - return total_time - t = time.localtime(m._timestamp) - if m.fix_type == 2 and not locked: - print("Locked at %s after %u seconds" % (time.asctime(t), - time.mktime(t) - unlock_time)) - locked = True - start_time = time.mktime(t) - elif m.fix_type == 1 and locked: - print("Lost GPS lock at %s" % time.asctime(t)) - locked = False - total_time += time.mktime(t) - start_time - unlock_time = time.mktime(t) - elif m.fix_type == 0 and locked: - print("Lost protocol lock at %s" % time.asctime(t)) - locked = False - total_time += time.mktime(t) - start_time - unlock_time = time.mktime(t) - return total_time - -total = 0.0 -for filename in args: - total += lock_time(filename) - -print("Total time locked: %u:%02u" % (int(total)/60, int(total)%60)) diff --git a/mavlink/share/pyshared/pymavlink/examples/magfit.py b/mavlink/share/pyshared/pymavlink/examples/magfit.py deleted file mode 100644 index 7bfda796b..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/magfit.py +++ /dev/null @@ -1,138 +0,0 @@ -#!/usr/bin/env python - -''' -fit best estimate of magnetometer offsets -''' - -import sys, time, os, math - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -from optparse import OptionParser -parser = OptionParser("magfit.py [options]") -parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") -parser.add_option("--condition",dest="condition", default=None, help="select packets by condition") -parser.add_option("--noise", type='float', default=0, help="noise to add") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") - -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil -from rotmat import Vector3 - -if len(args) < 1: - print("Usage: magfit.py [options] ") - sys.exit(1) - -def noise(): - '''a noise vector''' - from random import gauss - v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1)) - v.normalize() - return v * opts.noise - -def select_data(data): - ret = [] - counts = {} - for d in data: - mag = d - key = "%u:%u:%u" % (mag.x/20,mag.y/20,mag.z/20) - if key in counts: - counts[key] += 1 - else: - counts[key] = 1 - if counts[key] < 3: - ret.append(d) - print(len(data), len(ret)) - return ret - -def radius(mag, offsets): - '''return radius give data point and offsets''' - return (mag + offsets).length() - -def radius_cmp(a, b, offsets): - '''return radius give data point and offsets''' - diff = radius(a, offsets) - radius(b, offsets) - if diff > 0: - return 1 - if diff < 0: - return -1 - return 0 - -def sphere_error(p, data): - from scipy import sqrt - x,y,z,r = p - ofs = Vector3(x,y,z) - ret = [] - for d in data: - mag = d - err = r - radius(mag, ofs) - ret.append(err) - return ret - -def fit_data(data): - import numpy, scipy - from scipy import optimize - - p0 = [0.0, 0.0, 0.0, 0.0] - p1, ier = optimize.leastsq(sphere_error, p0[:], args=(data)) - if not ier in [1, 2, 3, 4]: - raise RuntimeError("Unable to find solution") - return (Vector3(p1[0], p1[1], p1[2]), p1[3]) - -def magfit(logfile): - '''find best magnetometer offset fit to a log file''' - - print("Processing log %s" % filename) - mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps) - - data = [] - - last_t = 0 - offsets = Vector3(0,0,0) - - # now gather all the data - while True: - m = mlog.recv_match(condition=opts.condition) - if m is None: - break - if m.get_type() == "SENSOR_OFFSETS": - # update current offsets - offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) - if m.get_type() == "RAW_IMU": - mag = Vector3(m.xmag, m.ymag, m.zmag) - # add data point after subtracting the current offsets - data.append(mag - offsets + noise()) - - print("Extracted %u data points" % len(data)) - print("Current offsets: %s" % offsets) - - data = select_data(data) - - # do an initial fit with all data - (offsets, field_strength) = fit_data(data) - - for count in range(3): - # sort the data by the radius - data.sort(lambda a,b : radius_cmp(a,b,offsets)) - - print("Fit %u : %s field_strength=%6.1f to %6.1f" % ( - count, offsets, - radius(data[0], offsets), radius(data[-1], offsets))) - - # discard outliers, keep the middle 3/4 - data = data[len(data)/8:-len(data)/8] - - # fit again - (offsets, field_strength) = fit_data(data) - - print("Final : %s field_strength=%6.1f to %6.1f" % ( - offsets, - radius(data[0], offsets), radius(data[-1], offsets))) - -total = 0.0 -for filename in args: - magfit(filename) diff --git a/mavlink/share/pyshared/pymavlink/examples/magfit_delta.py b/mavlink/share/pyshared/pymavlink/examples/magfit_delta.py deleted file mode 100644 index 87d2dbb7f..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/magfit_delta.py +++ /dev/null @@ -1,145 +0,0 @@ -#!/usr/bin/env python - -''' -fit best estimate of magnetometer offsets using the algorithm from -Bill Premerlani -''' - -import sys, time, os, math - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -# command line option handling -from optparse import OptionParser -parser = OptionParser("magfit_delta.py [options]") -parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") -parser.add_option("--condition",dest="condition", default=None, help="select packets by condition") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -parser.add_option("--verbose", action='store_true', default=False, help="verbose offset output") -parser.add_option("--gain", type='float', default=0.01, help="algorithm gain") -parser.add_option("--noise", type='float', default=0, help="noise to add") -parser.add_option("--max-change", type='float', default=10, help="max step change") -parser.add_option("--min-diff", type='float', default=50, help="min mag vector delta") -parser.add_option("--history", type='int', default=20, help="how many points to keep") -parser.add_option("--repeat", type='int', default=1, help="number of repeats through the data") - -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil -from rotmat import Vector3, Matrix3 - -if len(args) < 1: - print("Usage: magfit_delta.py [options] ") - sys.exit(1) - -def noise(): - '''a noise vector''' - from random import gauss - v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1)) - v.normalize() - return v * opts.noise - -def find_offsets(data, ofs): - '''find mag offsets by applying Bills "offsets revisited" algorithm - on the data - - This is an implementation of the algorithm from: - http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf - ''' - - # a limit on the maximum change in each step - max_change = opts.max_change - - # the gain factor for the algorithm - gain = opts.gain - - data2 = [] - for d in data: - d = d.copy() + noise() - d.x = float(int(d.x + 0.5)) - d.y = float(int(d.y + 0.5)) - d.z = float(int(d.z + 0.5)) - data2.append(d) - data = data2 - - history_idx = 0 - mag_history = data[0:opts.history] - - for i in range(opts.history, len(data)): - B1 = mag_history[history_idx] + ofs - B2 = data[i] + ofs - - diff = B2 - B1 - diff_length = diff.length() - if diff_length <= opts.min_diff: - # the mag vector hasn't changed enough - we don't get any - # information from this - history_idx = (history_idx+1) % opts.history - continue - - mag_history[history_idx] = data[i] - history_idx = (history_idx+1) % opts.history - - # equation 6 of Bills paper - delta = diff * (gain * (B2.length() - B1.length()) / diff_length) - - # limit the change from any one reading. This is to prevent - # single crazy readings from throwing off the offsets for a long - # time - delta_length = delta.length() - if max_change != 0 and delta_length > max_change: - delta *= max_change / delta_length - - # set the new offsets - ofs = ofs - delta - - if opts.verbose: - print ofs - return ofs - - -def magfit(logfile): - '''find best magnetometer offset fit to a log file''' - - print("Processing log %s" % filename) - - # open the log file - mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps) - - data = [] - mag = None - offsets = Vector3(0,0,0) - - # now gather all the data - while True: - # get the next MAVLink message in the log - m = mlog.recv_match(condition=opts.condition) - if m is None: - break - if m.get_type() == "SENSOR_OFFSETS": - # update offsets that were used during this flight - offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) - if m.get_type() == "RAW_IMU" and offsets != None: - # extract one mag vector, removing the offsets that were - # used during that flight to get the raw sensor values - mag = Vector3(m.xmag, m.ymag, m.zmag) - offsets - data.append(mag) - - print("Extracted %u data points" % len(data)) - print("Current offsets: %s" % offsets) - - # run the fitting algorithm - ofs = offsets - ofs = Vector3(0,0,0) - for r in range(opts.repeat): - ofs = find_offsets(data, ofs) - print('Loop %u offsets %s' % (r, ofs)) - sys.stdout.flush() - print("New offsets: %s" % ofs) - -total = 0.0 -for filename in args: - magfit(filename) diff --git a/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py b/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py deleted file mode 100644 index 30ba45806..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py +++ /dev/null @@ -1,159 +0,0 @@ -#!/usr/bin/env python - -''' -fit best estimate of magnetometer offsets -''' - -import sys, time, os, math - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -from optparse import OptionParser -parser = OptionParser("magfit.py [options]") -parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -parser.add_option("--minspeed", type='float', default=5.0, help="minimum ground speed to use") - -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 1: - print("Usage: magfit.py [options] ") - sys.exit(1) - -class vec3(object): - def __init__(self, x, y, z): - self.x = x - self.y = y - self.z = z - def __str__(self): - return "%.1f %.1f %.1f" % (self.x, self.y, self.z) - -def heading_error1(parm, data): - from math import sin, cos, atan2, degrees - from numpy import dot - xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm - - ret = [] - for d in data: - x = d[0] + xofs - y = d[1] + yofs - z = d[2] + zofs - r = d[3] - p = d[4] - h = d[5] - - headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p) - headY = y*cos(r) - z*sin(r) - heading = degrees(atan2(-headY,headX)) + declination - if heading < 0: - heading += 360 - herror = h - heading - if herror > 180: - herror -= 360 - if herror < -180: - herror += 360 - ret.append(herror) - return ret - -def heading_error(parm, data): - from math import sin, cos, atan2, degrees - from numpy import dot - xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm - - a = [[1.0,a2,a3],[a4,a5,a6],[a7,a8,a9]] - - ret = [] - for d in data: - x = d[0] + xofs - y = d[1] + yofs - z = d[2] + zofs - r = d[3] - p = d[4] - h = d[5] - mv = [x, y, z] - mv2 = dot(a, mv) - x = mv2[0] - y = mv2[1] - z = mv2[2] - - headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p) - headY = y*cos(r) - z*sin(r) - heading = degrees(atan2(-headY,headX)) + declination - if heading < 0: - heading += 360 - herror = h - heading - if herror > 180: - herror -= 360 - if herror < -180: - herror += 360 - ret.append(herror) - return ret - -def fit_data(data): - import numpy, scipy - from scipy import optimize - - p0 = [0.0, 0.0, 0.0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0] - p1, ier = optimize.leastsq(heading_error1, p0[:], args=(data)) - -# p0 = p1[:] -# p1, ier = optimize.leastsq(heading_error, p0[:], args=(data)) - - print(p1) - if not ier in [1, 2, 3, 4]: - raise RuntimeError("Unable to find solution") - return p1 - -def magfit(logfile): - '''find best magnetometer offset fit to a log file''' - print("Processing log %s" % filename) - mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps) - - flying = False - gps_heading = 0.0 - - data = [] - - # get the current mag offsets - m = mlog.recv_match(type='SENSOR_OFFSETS') - offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) - - attitude = mlog.recv_match(type='ATTITUDE') - - # now gather all the data - while True: - m = mlog.recv_match() - if m is None: - break - if m.get_type() == "GPS_RAW": - # flying if groundspeed more than 5 m/s - flying = (m.v > opts.minspeed and m.fix_type == 2) - gps_heading = m.hdg - if m.get_type() == "ATTITUDE": - attitude = m - if m.get_type() == "SENSOR_OFFSETS": - # update current offsets - offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) - if not flying: - continue - if m.get_type() == "RAW_IMU": - data.append((m.xmag - offsets.x, m.ymag - offsets.y, m.zmag - offsets.z, attitude.roll, attitude.pitch, gps_heading)) - print("Extracted %u data points" % len(data)) - print("Current offsets: %s" % offsets) - ofs2 = fit_data(data) - print("Declination estimate: %.1f" % ofs2[-1]) - new_offsets = vec3(ofs2[0], ofs2[1], ofs2[2]) - a = [[ofs2[3], ofs2[4], ofs2[5]], - [ofs2[6], ofs2[7], ofs2[8]], - [ofs2[9], ofs2[10], ofs2[11]]] - print(a) - print("New offsets : %s" % new_offsets) - -total = 0.0 -for filename in args: - magfit(filename) diff --git a/mavlink/share/pyshared/pymavlink/examples/magtest.py b/mavlink/share/pyshared/pymavlink/examples/magtest.py deleted file mode 100644 index 8b910f8fd..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/magtest.py +++ /dev/null @@ -1,120 +0,0 @@ -#!/usr/bin/env python - -''' -rotate APMs on bench to test magnetometers - -''' - -import sys, os, time -from math import radians - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -import mavlink, mavutil - -from optparse import OptionParser -parser = OptionParser("rotate.py [options]") - -parser.add_option("--device1", dest="device1", default=None, help="mavlink device1") -parser.add_option("--device2", dest="device2", default=None, help="mavlink device2") -parser.add_option("--baudrate", dest="baudrate", type='int', - help="master port baud rate", default=115200) -(opts, args) = parser.parse_args() - -if opts.device1 is None or opts.device2 is None: - print("You must specify a mavlink device") - sys.exit(1) - -def set_attitude(rc3, rc4): - global mav1, mav2 - values = [ 65535 ] * 8 - values[2] = rc3 - values[3] = rc4 - mav1.mav.rc_channels_override_send(mav1.target_system, mav1.target_component, *values) - mav2.mav.rc_channels_override_send(mav2.target_system, mav2.target_component, *values) - - -# create a mavlink instance -mav1 = mavutil.mavlink_connection(opts.device1, baud=opts.baudrate) - -# create a mavlink instance -mav2 = mavutil.mavlink_connection(opts.device2, baud=opts.baudrate) - -print("Waiting for HEARTBEAT") -mav1.wait_heartbeat() -mav2.wait_heartbeat() -print("Heartbeat from APM (system %u component %u)" % (mav1.target_system, mav1.target_system)) -print("Heartbeat from APM (system %u component %u)" % (mav2.target_system, mav2.target_system)) - -print("Waiting for MANUAL mode") -mav1.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True) -mav2.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True) - -print("Setting declination") -mav1.mav.param_set_send(mav1.target_system, mav1.target_component, - 'COMPASS_DEC', radians(12.33)) -mav2.mav.param_set_send(mav2.target_system, mav2.target_component, - 'COMPASS_DEC', radians(12.33)) - - -set_attitude(1060, 1160) - -event = mavutil.periodic_event(30) -pevent = mavutil.periodic_event(0.3) -rc3_min = 1060 -rc3_max = 1850 -rc4_min = 1080 -rc4_max = 1500 -rc3 = rc3_min -rc4 = 1160 -delta3 = 2 -delta4 = 1 -use_pitch = 1 - -MAV_ACTION_CALIBRATE_GYRO = 17 -mav1.mav.action_send(mav1.target_system, mav1.target_component, MAV_ACTION_CALIBRATE_GYRO) -mav2.mav.action_send(mav2.target_system, mav2.target_component, MAV_ACTION_CALIBRATE_GYRO) - -print("Waiting for gyro calibration") -mav1.recv_match(type='ACTION_ACK') -mav2.recv_match(type='ACTION_ACK') - -print("Resetting mag offsets") -mav1.mav.set_mag_offsets_send(mav1.target_system, mav1.target_component, 0, 0, 0) -mav2.mav.set_mag_offsets_send(mav2.target_system, mav2.target_component, 0, 0, 0) - -def TrueHeading(SERVO_OUTPUT_RAW): - p = float(SERVO_OUTPUT_RAW.servo3_raw - rc3_min) / (rc3_max - rc3_min) - return 172 + p*(326 - 172) - -while True: - mav1.recv_msg() - mav2.recv_msg() - if event.trigger(): - if not use_pitch: - rc4 = 1160 - set_attitude(rc3, rc4) - rc3 += delta3 - if rc3 > rc3_max or rc3 < rc3_min: - delta3 = -delta3 - use_pitch ^= 1 - rc4 += delta4 - if rc4 > rc4_max or rc4 < rc4_min: - delta4 = -delta4 - if pevent.trigger(): - print "hdg1: %3u hdg2: %3u ofs1: %4u, %4u, %4u ofs2: %4u, %4u, %4u" % ( - mav1.messages['VFR_HUD'].heading, - mav2.messages['VFR_HUD'].heading, - mav1.messages['SENSOR_OFFSETS'].mag_ofs_x, - mav1.messages['SENSOR_OFFSETS'].mag_ofs_y, - mav1.messages['SENSOR_OFFSETS'].mag_ofs_z, - mav2.messages['SENSOR_OFFSETS'].mag_ofs_x, - mav2.messages['SENSOR_OFFSETS'].mag_ofs_y, - mav2.messages['SENSOR_OFFSETS'].mag_ofs_z, - ) - time.sleep(0.01) - -# 314M 326G -# 160M 172G - diff --git a/mavlink/share/pyshared/pymavlink/examples/mavgraph.py b/mavlink/share/pyshared/pymavlink/examples/mavgraph.py deleted file mode 100644 index e19856487..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/mavgraph.py +++ /dev/null @@ -1,196 +0,0 @@ -#!/usr/bin/env python -''' -graph a MAVLink log file -Andrew Tridgell August 2011 -''' - -import sys, struct, time, os, datetime -import math, re -import pylab, pytz, matplotlib -from math import * - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -from mavextra import * - -locator = None -formatter = None - -def plotit(x, y, fields, colors=[]): - '''plot a set of graphs using date for x axis''' - global locator, formatter - pylab.ion() - fig = pylab.figure(num=1, figsize=(12,6)) - ax1 = fig.gca() - ax2 = None - xrange = 0.0 - for i in range(0, len(fields)): - if len(x[i]) == 0: continue - if x[i][-1] - x[i][0] > xrange: - xrange = x[i][-1] - x[i][0] - xrange *= 24 * 60 * 60 - if formatter is None: - if xrange < 1000: - formatter = matplotlib.dates.DateFormatter('%H:%M:%S') - else: - formatter = matplotlib.dates.DateFormatter('%H:%M') - interval = 1 - intervals = [ 1, 2, 5, 10, 15, 30, 60, 120, 240, 300, 600, - 900, 1800, 3600, 7200, 5*3600, 10*3600, 24*3600 ] - for interval in intervals: - if xrange / interval < 15: - break - locator = matplotlib.dates.SecondLocator(interval=interval) - ax1.xaxis.set_major_locator(locator) - ax1.xaxis.set_major_formatter(formatter) - empty = True - ax1_labels = [] - ax2_labels = [] - for i in range(0, len(fields)): - if len(x[i]) == 0: - print("Failed to find any values for field %s" % fields[i]) - continue - if i < len(colors): - color = colors[i] - else: - color = 'red' - (tz, tzdst) = time.tzname - if axes[i] == 2: - if ax2 == None: - ax2 = ax1.twinx() - ax = ax2 - ax2.xaxis.set_major_locator(locator) - ax2.xaxis.set_major_formatter(formatter) - label = fields[i] - if label.endswith(":2"): - label = label[:-2] - ax2_labels.append(label) - else: - ax1_labels.append(fields[i]) - ax = ax1 - ax.plot_date(x[i], y[i], color=color, label=fields[i], - linestyle='-', marker='None', tz=None) - pylab.draw() - empty = False - if ax1_labels != []: - ax1.legend(ax1_labels,loc=opts.legend) - if ax2_labels != []: - ax2.legend(ax2_labels,loc=opts.legend2) - if empty: - print("No data to graph") - return - - -from optparse import OptionParser -parser = OptionParser("mavgraph.py [options] ") - -parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") -parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format") -parser.add_option("--condition",dest="condition", default=None, help="select packets by a condition") -parser.add_option("--labels",dest="labels", default=None, help="comma separated field labels") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -parser.add_option("--legend", default='upper left', help="default legend position") -parser.add_option("--legend2", default='upper right', help="default legend2 position") -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 2: - print("Usage: mavlogdump.py [options] ") - sys.exit(1) - -filenames = [] -fields = [] -for f in args: - if os.path.exists(f): - filenames.append(f) - else: - fields.append(f) -msg_types = set() -multiplier = [] -field_types = [] - -colors = [ 'red', 'green', 'blue', 'orange', 'olive', 'black', 'grey' ] - -# work out msg types we are interested in -x = [] -y = [] -axes = [] -first_only = [] -re_caps = re.compile('[A-Z_]+') -for f in fields: - caps = set(re.findall(re_caps, f)) - msg_types = msg_types.union(caps) - field_types.append(caps) - y.append([]) - x.append([]) - axes.append(1) - first_only.append(False) - -def add_data(t, msg, vars): - '''add some data''' - mtype = msg.get_type() - if mtype not in msg_types: - return - for i in range(0, len(fields)): - if mtype not in field_types[i]: - continue - f = fields[i] - if f.endswith(":2"): - axes[i] = 2 - f = f[:-2] - if f.endswith(":1"): - first_only[i] = True - f = f[:-2] - v = mavutil.evaluate_expression(f, vars) - if v is None: - continue - y[i].append(v) - x[i].append(t) - -def process_file(filename): - '''process one file''' - print("Processing %s" % filename) - mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps) - vars = {} - - while True: - msg = mlog.recv_match(opts.condition) - if msg is None: break - tdays = (msg._timestamp - time.timezone) / (24 * 60 * 60) - tdays += 719163 # pylab wants it since 0001-01-01 - add_data(tdays, msg, mlog.messages) - -if len(filenames) == 0: - print("No files to process") - sys.exit(1) - -if opts.labels is not None: - labels = opts.labels.split(',') - if len(labels) != len(fields)*len(filenames): - print("Number of labels (%u) must match number of fields (%u)" % ( - len(labels), len(fields)*len(filenames))) - sys.exit(1) -else: - labels = None - -for fi in range(0, len(filenames)): - f = filenames[fi] - process_file(f) - for i in range(0, len(x)): - if first_only[i] and fi != 0: - x[i] = [] - y[i] = [] - if labels: - lab = labels[fi*len(fields):(fi+1)*len(fields)] - else: - lab = fields[:] - plotit(x, y, lab, colors=colors[fi*len(fields):]) - for i in range(0, len(x)): - x[i] = [] - y[i] = [] -pylab.show() -raw_input('press enter to exit....') diff --git a/mavlink/share/pyshared/pymavlink/examples/mavlogdump.py b/mavlink/share/pyshared/pymavlink/examples/mavlogdump.py deleted file mode 100644 index f4cdc56bf..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/mavlogdump.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python - -''' -example program that dumps a Mavlink log file. The log file is -assumed to be in the format that qgroundcontrol uses, which consists -of a series of MAVLink packets, each with a 64 bit timestamp -header. The timestamp is in microseconds since 1970 (unix epoch) -''' - -import sys, time, os, struct - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -from optparse import OptionParser -parser = OptionParser("mavlogdump.py [options]") - -parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") -parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format") -parser.add_option("--robust",dest="robust", action='store_true', help="Enable robust parsing (skip over bad data)") -parser.add_option("-f", "--follow",dest="follow", action='store_true', help="keep waiting for more data at end of file") -parser.add_option("--condition",dest="condition", default=None, help="select packets by condition") -parser.add_option("-q", "--quiet", dest="quiet", action='store_true', help="don't display packets") -parser.add_option("-o", "--output", default=None, help="output matching packets to give file") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -parser.add_option("--types", default=None, help="types of messages (comma separated)") -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 1: - print("Usage: mavlogdump.py [options] ") - sys.exit(1) - -filename = args[0] -mlog = mavutil.mavlink_connection(filename, planner_format=opts.planner, - notimestamps=opts.notimestamps, - robust_parsing=opts.robust) - -output = None -if opts.output: - output = mavutil.mavlogfile(opts.output, write=True) - -types = opts.types -if types is not None: - types = types.split(',') - -while True: - m = mlog.recv_match(condition=opts.condition, blocking=opts.follow) - if m is None: - break - if types is not None and m.get_type() not in types: - continue - if output: - timestamp = getattr(m, '_timestamp', None) - if timestamp: - output.write(struct.pack('>Q', timestamp*1.0e6)) - output.write(m.get_msgbuf().tostring()) - if opts.quiet: - continue - print("%s.%02u: %s" % ( - time.strftime("%Y-%m-%d %H:%M:%S", - time.localtime(m._timestamp)), - int(m._timestamp*100.0)%100, m)) - diff --git a/mavlink/share/pyshared/pymavlink/examples/mavparms.py b/mavlink/share/pyshared/pymavlink/examples/mavparms.py deleted file mode 100644 index 702fbd9e1..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/mavparms.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python - -''' -extract mavlink parameter values -''' - -import sys, time, os - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -from optparse import OptionParser -parser = OptionParser("mavparms.py [options]") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") - -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 1: - print("Usage: mavparms.py [options] ") - sys.exit(1) - -parms = {} - -def mavparms(logfile): - '''extract mavlink parameters''' - mlog = mavutil.mavlink_connection(filename) - - while True: - m = mlog.recv_match(type='PARAM_VALUE') - if m is None: - return - pname = str(m.param_id).strip() - if len(pname) > 0: - parms[pname] = m.param_value - -total = 0.0 -for filename in args: - mavparms(filename) - -keys = parms.keys() -keys.sort() -for p in keys: - print("%-15s %.6f" % (p, parms[p])) - diff --git a/mavlink/share/pyshared/pymavlink/examples/mavtest.py b/mavlink/share/pyshared/pymavlink/examples/mavtest.py deleted file mode 100644 index 3c385e48a..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/mavtest.py +++ /dev/null @@ -1,41 +0,0 @@ -#!/usr/bin/env python - -import sys, os - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -import mavlink - -class fifo(object): - def __init__(self): - self.buf = [] - def write(self, data): - self.buf += data - return len(data) - def read(self): - return self.buf.pop(0) - -f = fifo() - -# create a mavlink instance, which will do IO on file object 'f' -mav = mavlink.MAVLink(f) - -# set the WP_RADIUS parameter on the MAV at the end of the link -mav.param_set_send(7, 1, "WP_RADIUS", 101) - -# alternatively, produce a MAVLink_param_set object -# this can be sent via your own transport if you like -m = mav.param_set_encode(7, 1, "WP_RADIUS", 101) - -# get the encoded message as a buffer -b = m.get_msgbuf() - -# decode an incoming message -m2 = mav.decode(b) - -# show what fields it has -print("Got a message with id %u and fields %s" % (m2.get_msgId(), m2.get_fieldnames())) - -# print out the fields -print(m2) diff --git a/mavlink/share/pyshared/pymavlink/examples/mavtester.py b/mavlink/share/pyshared/pymavlink/examples/mavtester.py deleted file mode 100644 index 8b5284f3f..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/mavtester.py +++ /dev/null @@ -1,43 +0,0 @@ -#!/usr/bin/env python - -''' -test mavlink messages -''' - -import sys, struct, time, os -from curses import ascii - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -import mavlink, mavtest, mavutil - -from optparse import OptionParser -parser = OptionParser("mavtester.py [options]") - -parser.add_option("--baudrate", dest="baudrate", type='int', - help="master port baud rate", default=115200) -parser.add_option("--device", dest="device", default=None, help="serial device") -parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int', - default=255, help='MAVLink source system for this GCS') -(opts, args) = parser.parse_args() - -if opts.device is None: - print("You must specify a serial device") - sys.exit(1) - -def wait_heartbeat(m): - '''wait for a heartbeat so we know the target system IDs''' - print("Waiting for APM heartbeat") - msg = m.recv_match(type='HEARTBEAT', blocking=True) - print("Heartbeat from APM (system %u component %u)" % (m.target_system, m.target_system)) - -# create a mavlink serial instance -master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate, source_system=opts.SOURCE_SYSTEM) - -# wait for the heartbeat msg to find the system ID -wait_heartbeat(master) - -print("Sending all message types") -mavtest.generate_outputs(master.mav) - diff --git a/mavlink/share/pyshared/pymavlink/examples/mavtogpx.py b/mavlink/share/pyshared/pymavlink/examples/mavtogpx.py deleted file mode 100644 index 92d3cb51c..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/mavtogpx.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/usr/bin/env python - -''' -example program to extract GPS data from a mavlink log, and create a GPX -file, for loading into google earth -''' - -import sys, struct, time, os - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -from optparse import OptionParser -parser = OptionParser("mavtogpx.py [options]") -parser.add_option("--condition",dest="condition", default=None, help="select packets by a condition") -parser.add_option("--nofixcheck", default=False, action='store_true', help="don't check for GPS fix") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 1: - print("Usage: mavtogpx.py ") - sys.exit(1) - -def mav_to_gpx(infilename, outfilename): - '''convert a mavlink log file to a GPX file''' - - mlog = mavutil.mavlink_connection(infilename) - outf = open(outfilename, mode='w') - - def process_packet(m): - t = time.localtime(m._timestamp) - outf.write(''' - %s - - %s - %s - 3d - -''' % (m.lat, m.lon, m.alt, - time.strftime("%Y-%m-%dT%H:%M:%SZ", t), - m.hdg, m.v)) - - def add_header(): - outf.write(''' - - - -''') - - def add_footer(): - outf.write(''' - - -''') - - add_header() - - count=0 - while True: - m = mlog.recv_match(type='GPS_RAW', condition=opts.condition) - if m is None: break - if m.fix_type != 2 and not opts.nofixcheck: - continue - if m.lat == 0.0 or m.lon == 0.0: - continue - process_packet(m) - count += 1 - add_footer() - print("Created %s with %u points" % (outfilename, count)) - - -for infilename in args: - outfilename = infilename + '.gpx' - mav_to_gpx(infilename, outfilename) diff --git a/mavlink/share/pyshared/pymavlink/examples/rotmat.py b/mavlink/share/pyshared/pymavlink/examples/rotmat.py deleted file mode 100644 index 6d5405949..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/rotmat.py +++ /dev/null @@ -1,269 +0,0 @@ -#!/usr/bin/env python -# -# vector3 and rotation matrix classes -# This follows the conventions in the ArduPilot code, -# and is essentially a python version of the AP_Math library -# -# Andrew Tridgell, March 2012 -# -# This library is free software; you can redistribute it and/or modify it -# under the terms of the GNU Lesser General Public License as published by the -# Free Software Foundation; either version 2.1 of the License, or (at your -# option) any later version. -# -# This library is distributed in the hope that it will be useful, but WITHOUT -# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -# FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License -# for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with this library; if not, write to the Free Software Foundation, -# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - -'''rotation matrix class -''' - -from math import sin, cos, sqrt, asin, atan2, pi, radians, acos - -class Vector3: - '''a vector''' - def __init__(self, x=None, y=None, z=None): - if x != None and y != None and z != None: - self.x = float(x) - self.y = float(y) - self.z = float(z) - elif x != None and len(x) == 3: - self.x = float(x[0]) - self.y = float(x[1]) - self.z = float(x[2]) - elif x != None: - raise ValueError('bad initialiser') - else: - self.x = float(0) - self.y = float(0) - self.z = float(0) - - def __repr__(self): - return 'Vector3(%.2f, %.2f, %.2f)' % (self.x, - self.y, - self.z) - - def __add__(self, v): - return Vector3(self.x + v.x, - self.y + v.y, - self.z + v.z) - - __radd__ = __add__ - - def __sub__(self, v): - return Vector3(self.x - v.x, - self.y - v.y, - self.z - v.z) - - def __neg__(self): - return Vector3(-self.x, -self.y, -self.z) - - def __rsub__(self, v): - return Vector3(v.x - self.x, - v.y - self.y, - v.z - self.z) - - def __mul__(self, v): - if isinstance(v, Vector3): - '''dot product''' - return self.x*v.x + self.y*v.y + self.z*v.z - return Vector3(self.x * v, - self.y * v, - self.z * v) - - __rmul__ = __mul__ - - def __div__(self, v): - return Vector3(self.x / v, - self.y / v, - self.z / v) - - def __mod__(self, v): - '''cross product''' - return Vector3(self.y*v.z - self.z*v.y, - self.z*v.x - self.x*v.z, - self.x*v.y - self.y*v.x) - - def __copy__(self): - return Vector3(self.x, self.y, self.z) - - copy = __copy__ - - def length(self): - return sqrt(self.x**2 + self.y**2 + self.z**2) - - def zero(self): - self.x = self.y = self.z = 0 - - def angle(self, v): - '''return the angle between this vector and another vector''' - return acos(self * v) / (self.length() * v.length()) - - def normalized(self): - return self / self.length() - - def normalize(self): - v = self.normalized() - self.x = v.x - self.y = v.y - self.z = v.z - -class Matrix3: - '''a 3x3 matrix, intended as a rotation matrix''' - def __init__(self, a=None, b=None, c=None): - if a is not None and b is not None and c is not None: - self.a = a.copy() - self.b = b.copy() - self.c = c.copy() - else: - self.identity() - - def __repr__(self): - return 'Matrix3((%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f))' % ( - self.a.x, self.a.y, self.a.z, - self.b.x, self.b.y, self.b.z, - self.c.x, self.c.y, self.c.z) - - def identity(self): - self.a = Vector3(1,0,0) - self.b = Vector3(0,1,0) - self.c = Vector3(0,0,1) - - def transposed(self): - return Matrix3(Vector3(self.a.x, self.b.x, self.c.x), - Vector3(self.a.y, self.b.y, self.c.y), - Vector3(self.a.z, self.b.z, self.c.z)) - - - def from_euler(self, roll, pitch, yaw): - '''fill the matrix from Euler angles in radians''' - cp = cos(pitch) - sp = sin(pitch) - sr = sin(roll) - cr = cos(roll) - sy = sin(yaw) - cy = cos(yaw) - - self.a.x = cp * cy - self.a.y = (sr * sp * cy) - (cr * sy) - self.a.z = (cr * sp * cy) + (sr * sy) - self.b.x = cp * sy - self.b.y = (sr * sp * sy) + (cr * cy) - self.b.z = (cr * sp * sy) - (sr * cy) - self.c.x = -sp - self.c.y = sr * cp - self.c.z = cr * cp - - - def to_euler(self): - '''find Euler angles for the matrix''' - if self.c.x >= 1.0: - pitch = pi - elif self.c.x <= -1.0: - pitch = -pi - else: - pitch = -asin(self.c.x) - roll = atan2(self.c.y, self.c.z) - yaw = atan2(self.b.x, self.a.x) - return (roll, pitch, yaw) - - def __add__(self, m): - return Matrix3(self.a + m.a, self.b + m.b, self.c + m.c) - - __radd__ = __add__ - - def __sub__(self, m): - return Matrix3(self.a - m.a, self.b - m.b, self.c - m.c) - - def __rsub__(self, m): - return Matrix3(m.a - self.a, m.b - self.b, m.c - self.c) - - def __mul__(self, other): - if isinstance(other, Vector3): - v = other - return Vector3(self.a.x * v.x + self.a.y * v.y + self.a.z * v.z, - self.b.x * v.x + self.b.y * v.y + self.b.z * v.z, - self.c.x * v.x + self.c.y * v.y + self.c.z * v.z) - elif isinstance(other, Matrix3): - m = other - return Matrix3(Vector3(self.a.x * m.a.x + self.a.y * m.b.x + self.a.z * m.c.x, - self.a.x * m.a.y + self.a.y * m.b.y + self.a.z * m.c.y, - self.a.x * m.a.z + self.a.y * m.b.z + self.a.z * m.c.z), - Vector3(self.b.x * m.a.x + self.b.y * m.b.x + self.b.z * m.c.x, - self.b.x * m.a.y + self.b.y * m.b.y + self.b.z * m.c.y, - self.b.x * m.a.z + self.b.y * m.b.z + self.b.z * m.c.z), - Vector3(self.c.x * m.a.x + self.c.y * m.b.x + self.c.z * m.c.x, - self.c.x * m.a.y + self.c.y * m.b.y + self.c.z * m.c.y, - self.c.x * m.a.z + self.c.y * m.b.z + self.c.z * m.c.z)) - v = other - return Matrix3(self.a * v, self.b * v, self.c * v) - - def __div__(self, v): - return Matrix3(self.a / v, self.b / v, self.c / v) - - def __neg__(self): - return Matrix3(-self.a, -self.b, -self.c) - - def __copy__(self): - return Matrix3(self.a, self.b, self.c) - - copy = __copy__ - - def rotate(self, g): - '''rotate the matrix by a given amount on 3 axes''' - temp_matrix = Matrix3() - a = self.a - b = self.b - c = self.c - temp_matrix.a.x = a.y * g.z - a.z * g.y - temp_matrix.a.y = a.z * g.x - a.x * g.z - temp_matrix.a.z = a.x * g.y - a.y * g.x - temp_matrix.b.x = b.y * g.z - b.z * g.y - temp_matrix.b.y = b.z * g.x - b.x * g.z - temp_matrix.b.z = b.x * g.y - b.y * g.x - temp_matrix.c.x = c.y * g.z - c.z * g.y - temp_matrix.c.y = c.z * g.x - c.x * g.z - temp_matrix.c.z = c.x * g.y - c.y * g.x - self.a += temp_matrix.a - self.b += temp_matrix.b - self.c += temp_matrix.c - - def normalize(self): - '''re-normalise a rotation matrix''' - error = self.a * self.b - t0 = self.a - (self.b * (0.5 * error)) - t1 = self.b - (self.a * (0.5 * error)) - t2 = t0 % t1 - self.a = t0 * (1.0 / t0.length()) - self.b = t1 * (1.0 / t1.length()) - self.c = t2 * (1.0 / t2.length()) - - def trace(self): - '''the trace of the matrix''' - return self.a.x + self.b.y + self.c.z - -def test_euler(): - '''check that from_euler() and to_euler() are consistent''' - m = Matrix3() - from math import radians, degrees - for r in range(-179, 179, 3): - for p in range(-89, 89, 3): - for y in range(-179, 179, 3): - m.from_euler(radians(r), radians(p), radians(y)) - (r2, p2, y2) = m.to_euler() - v1 = Vector3(r,p,y) - v2 = Vector3(degrees(r2),degrees(p2),degrees(y2)) - diff = v1 - v2 - if diff.length() > 1.0e-12: - print('EULER ERROR:', v1, v2, diff.length()) - -if __name__ == "__main__": - import doctest - doctest.testmod() - test_euler() - diff --git a/mavlink/share/pyshared/pymavlink/examples/sigloss.py b/mavlink/share/pyshared/pymavlink/examples/sigloss.py deleted file mode 100644 index feb189d97..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/sigloss.py +++ /dev/null @@ -1,57 +0,0 @@ -#!/usr/bin/env python - -''' -show times when signal is lost -''' - -import sys, time, os - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -from optparse import OptionParser -parser = OptionParser("sigloss.py [options]") -parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") -parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format") -parser.add_option("--robust",dest="robust", action='store_true', help="Enable robust parsing (skip over bad data)") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -parser.add_option("--deltat", type='float', default=1.0, help="loss threshold in seconds") - -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 1: - print("Usage: sigloss.py [options] ") - sys.exit(1) - -def sigloss(logfile): - '''work out signal loss times for a log file''' - print("Processing log %s" % filename) - mlog = mavutil.mavlink_connection(filename, - planner_format=opts.planner, - notimestamps=opts.notimestamps, - robust_parsing=opts.robust) - - last_t = 0 - - while True: - m = mlog.recv_match() - if m is None: - return - if opts.notimestamps: - if not 'usec' in m._fieldnames: - continue - t = m.usec / 1.0e6 - else: - t = m._timestamp - if last_t != 0: - if t - last_t > opts.deltat: - print("Sig lost for %.1fs at %s" % (t-last_t, time.asctime(time.localtime(t)))) - last_t = t - -total = 0.0 -for filename in args: - sigloss(filename) diff --git a/mavlink/share/pyshared/pymavlink/examples/wptogpx.py b/mavlink/share/pyshared/pymavlink/examples/wptogpx.py deleted file mode 100644 index 306f20af2..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/wptogpx.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python - -''' -example program to extract GPS data from a waypoint file, and create a GPX -file, for loading into google earth -''' - -import sys, struct, time, os - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -from optparse import OptionParser -parser = OptionParser("wptogpx.py [options]") -(opts, args) = parser.parse_args() - -import mavutil, mavwp - -if len(args) < 1: - print("Usage: wptogpx.py ") - sys.exit(1) - -def wp_to_gpx(infilename, outfilename): - '''convert a wp file to a GPX file''' - - wp = mavwp.MAVWPLoader() - wp.load(infilename) - outf = open(outfilename, mode='w') - - def process_wp(w, i): - t = time.localtime(i) - outf.write(''' - %s - WP %u - -''' % (w.x, w.y, w.z, i)) - - def add_header(): - outf.write(''' - -''') - - def add_footer(): - outf.write(''' - -''') - - add_header() - - count = 0 - for i in range(wp.count()): - w = wp.wp(i) - if w.frame == 3: - w.z += wp.wp(0).z - if w.command == 16: - process_wp(w, i) - count += 1 - add_footer() - print("Created %s with %u points" % (outfilename, count)) - - -for infilename in args: - outfilename = infilename + '.gpx' - wp_to_gpx(infilename, outfilename) diff --git a/mavlink/share/pyshared/pymavlink/fgFDM.py b/mavlink/share/pyshared/pymavlink/fgFDM.py deleted file mode 100644 index f390e0a93..000000000 --- a/mavlink/share/pyshared/pymavlink/fgFDM.py +++ /dev/null @@ -1,209 +0,0 @@ -#!/usr/bin/env python -# parse and construct FlightGear NET FDM packets -# Andrew Tridgell, November 2011 -# released under GNU GPL version 2 or later - -import struct, math - -class fgFDMError(Exception): - '''fgFDM error class''' - def __init__(self, msg): - Exception.__init__(self, msg) - self.message = 'fgFDMError: ' + msg - -class fgFDMVariable(object): - '''represent a single fgFDM variable''' - def __init__(self, index, arraylength, units): - self.index = index - self.arraylength = arraylength - self.units = units - -class fgFDMVariableList(object): - '''represent a list of fgFDM variable''' - def __init__(self): - self.vars = {} - self._nextidx = 0 - - def add(self, varname, arraylength=1, units=None): - self.vars[varname] = fgFDMVariable(self._nextidx, arraylength, units=units) - self._nextidx += arraylength - -class fgFDM(object): - '''a flightgear native FDM parser/generator''' - def __init__(self): - '''init a fgFDM object''' - self.FG_NET_FDM_VERSION = 24 - self.pack_string = '>I 4x 3d 6f 11f 3f 2f I 4I 4f 4f 4f 4f 4f 4f 4f 4f 4f I 4f I 3I 3f 3f 3f I i f 10f' - self.values = [0]*98 - - self.FG_MAX_ENGINES = 4 - self.FG_MAX_WHEELS = 3 - self.FG_MAX_TANKS = 4 - - # supported unit mappings - self.unitmap = { - ('radians', 'degrees') : math.degrees(1), - ('rps', 'dps') : math.degrees(1), - ('feet', 'meters') : 0.3048, - ('fps', 'mps') : 0.3048, - ('knots', 'mps') : 0.514444444, - ('knots', 'fps') : 0.514444444/0.3048, - ('fpss', 'mpss') : 0.3048, - ('seconds', 'minutes') : 60, - ('seconds', 'hours') : 3600, - } - - # build a mapping between variable name and index in the values array - # note that the order of this initialisation is critical - it must - # match the wire structure - self.mapping = fgFDMVariableList() - self.mapping.add('version') - - # position - self.mapping.add('longitude', units='radians') # geodetic (radians) - self.mapping.add('latitude', units='radians') # geodetic (radians) - self.mapping.add('altitude', units='meters') # above sea level (meters) - self.mapping.add('agl', units='meters') # above ground level (meters) - - # attitude - self.mapping.add('phi', units='radians') # roll (radians) - self.mapping.add('theta', units='radians') # pitch (radians) - self.mapping.add('psi', units='radians') # yaw or true heading (radians) - self.mapping.add('alpha', units='radians') # angle of attack (radians) - self.mapping.add('beta', units='radians') # side slip angle (radians) - - # Velocities - self.mapping.add('phidot', units='rps') # roll rate (radians/sec) - self.mapping.add('thetadot', units='rps') # pitch rate (radians/sec) - self.mapping.add('psidot', units='rps') # yaw rate (radians/sec) - self.mapping.add('vcas', units='fps') # calibrated airspeed - self.mapping.add('climb_rate', units='fps') # feet per second - self.mapping.add('v_north', units='fps') # north velocity in local/body frame, fps - self.mapping.add('v_east', units='fps') # east velocity in local/body frame, fps - self.mapping.add('v_down', units='fps') # down/vertical velocity in local/body frame, fps - self.mapping.add('v_wind_body_north', units='fps') # north velocity in local/body frame - self.mapping.add('v_wind_body_east', units='fps') # east velocity in local/body frame - self.mapping.add('v_wind_body_down', units='fps') # down/vertical velocity in local/body - - # Accelerations - self.mapping.add('A_X_pilot', units='fpss') # X accel in body frame ft/sec^2 - self.mapping.add('A_Y_pilot', units='fpss') # Y accel in body frame ft/sec^2 - self.mapping.add('A_Z_pilot', units='fpss') # Z accel in body frame ft/sec^2 - - # Stall - self.mapping.add('stall_warning') # 0.0 - 1.0 indicating the amount of stall - self.mapping.add('slip_deg', units='degrees') # slip ball deflection - - # Engine status - self.mapping.add('num_engines') # Number of valid engines - self.mapping.add('eng_state', self.FG_MAX_ENGINES) # Engine state (off, cranking, running) - self.mapping.add('rpm', self.FG_MAX_ENGINES) # Engine RPM rev/min - self.mapping.add('fuel_flow', self.FG_MAX_ENGINES) # Fuel flow gallons/hr - self.mapping.add('fuel_px', self.FG_MAX_ENGINES) # Fuel pressure psi - self.mapping.add('egt', self.FG_MAX_ENGINES) # Exhuast gas temp deg F - self.mapping.add('cht', self.FG_MAX_ENGINES) # Cylinder head temp deg F - self.mapping.add('mp_osi', self.FG_MAX_ENGINES) # Manifold pressure - self.mapping.add('tit', self.FG_MAX_ENGINES) # Turbine Inlet Temperature - self.mapping.add('oil_temp', self.FG_MAX_ENGINES) # Oil temp deg F - self.mapping.add('oil_px', self.FG_MAX_ENGINES) # Oil pressure psi - - # Consumables - self.mapping.add('num_tanks') # Max number of fuel tanks - self.mapping.add('fuel_quantity', self.FG_MAX_TANKS) - - # Gear status - self.mapping.add('num_wheels') - self.mapping.add('wow', self.FG_MAX_WHEELS) - self.mapping.add('gear_pos', self.FG_MAX_WHEELS) - self.mapping.add('gear_steer', self.FG_MAX_WHEELS) - self.mapping.add('gear_compression', self.FG_MAX_WHEELS) - - # Environment - self.mapping.add('cur_time', units='seconds') # current unix time - self.mapping.add('warp', units='seconds') # offset in seconds to unix time - self.mapping.add('visibility', units='meters') # visibility in meters (for env. effects) - - # Control surface positions (normalized values) - self.mapping.add('elevator') - self.mapping.add('elevator_trim_tab') - self.mapping.add('left_flap') - self.mapping.add('right_flap') - self.mapping.add('left_aileron') - self.mapping.add('right_aileron') - self.mapping.add('rudder') - self.mapping.add('nose_wheel') - self.mapping.add('speedbrake') - self.mapping.add('spoilers') - - self._packet_size = struct.calcsize(self.pack_string) - - self.set('version', self.FG_NET_FDM_VERSION) - - if len(self.values) != self.mapping._nextidx: - raise fgFDMError('Invalid variable list in initialisation') - - def packet_size(self): - '''return expected size of FG FDM packets''' - return self._packet_size - - def convert(self, value, fromunits, tounits): - '''convert a value from one set of units to another''' - if fromunits == tounits: - return value - if (fromunits,tounits) in self.unitmap: - return value * self.unitmap[(fromunits,tounits)] - if (tounits,fromunits) in self.unitmap: - return value / self.unitmap[(tounits,fromunits)] - raise fgFDMError("unknown unit mapping (%s,%s)" % (fromunits, tounits)) - - - def units(self, varname): - '''return the default units of a variable''' - if not varname in self.mapping.vars: - raise fgFDMError('Unknown variable %s' % varname) - return self.mapping.vars[varname].units - - - def variables(self): - '''return a list of available variables''' - return sorted(self.mapping.vars.keys(), - key = lambda v : self.mapping.vars[v].index) - - - def get(self, varname, idx=0, units=None): - '''get a variable value''' - if not varname in self.mapping.vars: - raise fgFDMError('Unknown variable %s' % varname) - if idx >= self.mapping.vars[varname].arraylength: - raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % ( - varname, idx, self.mapping.vars[varname].arraylength)) - value = self.values[self.mapping.vars[varname].index + idx] - if units: - value = self.convert(value, self.mapping.vars[varname].units, units) - return value - - def set(self, varname, value, idx=0, units=None): - '''set a variable value''' - if not varname in self.mapping.vars: - raise fgFDMError('Unknown variable %s' % varname) - if idx >= self.mapping.vars[varname].arraylength: - raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % ( - varname, idx, self.mapping.vars[varname].arraylength)) - if units: - value = self.convert(value, units, self.mapping.vars[varname].units) - self.values[self.mapping.vars[varname].index + idx] = value - - def parse(self, buf): - '''parse a FD FDM buffer''' - try: - t = struct.unpack(self.pack_string, buf) - except struct.error, msg: - raise fgFDMError('unable to parse - %s' % msg) - self.values = list(t) - - def pack(self): - '''pack a FD FDM buffer from current values''' - for i in range(len(self.values)): - if math.isnan(self.values[i]): - self.values[i] = 0 - return struct.pack(self.pack_string, *self.values) diff --git a/mavlink/share/pyshared/pymavlink/generator/.gitignore b/mavlink/share/pyshared/pymavlink/generator/.gitignore deleted file mode 100644 index 0d20b6487..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/.gitignore +++ /dev/null @@ -1 +0,0 @@ -*.pyc diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/checksum.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/checksum.h deleted file mode 100644 index b70991f5a..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/checksum.h +++ /dev/null @@ -1,89 +0,0 @@ -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef _CHECKSUM_H_ -#define _CHECKSUM_H_ - - -/** - * - * CALCULATE THE CHECKSUM - * - */ - -#define X25_INIT_CRC 0xffff -#define X25_VALIDATE_CRC 0xf0b8 - -/** - * @brief Accumulate the X.25 CRC by adding one char at a time. - * - * The checksum function adds the hash of one char at a time to the - * 16 bit checksum (uint16_t). - * - * @param data new char to hash - * @param crcAccum the already accumulated checksum - **/ -static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) -{ - /*Accumulate one byte of data into the CRC*/ - uint8_t tmp; - - tmp = data ^ (uint8_t)(*crcAccum &0xff); - tmp ^= (tmp<<4); - *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); -} - -/** - * @brief Initiliaze the buffer for the X.25 CRC - * - * @param crcAccum the 16 bit X.25 CRC - */ -static inline void crc_init(uint16_t* crcAccum) -{ - *crcAccum = X25_INIT_CRC; -} - - -/** - * @brief Calculates the X.25 checksum on a byte buffer - * - * @param pBuffer buffer containing the byte array to hash - * @param length length of the byte array - * @return the checksum over the buffer bytes - **/ -static inline uint16_t crc_calculate(uint8_t* pBuffer, uint16_t length) -{ - uint16_t crcTmp; - crc_init(&crcTmp); - while (length--) { - crc_accumulate(*pBuffer++, &crcTmp); - } - return crcTmp; -} - -/** - * @brief Accumulate the X.25 CRC by adding an array of bytes - * - * The checksum function adds the hash of one char at a time to the - * 16 bit checksum (uint16_t). - * - * @param data new bytes to hash - * @param crcAccum the already accumulated checksum - **/ -static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length) -{ - const uint8_t *p = (const uint8_t *)pBuffer; - while (length--) { - crc_accumulate(*p++, crcAccum); - } -} - - - - -#endif /* _CHECKSUM_H_ */ - -#ifdef __cplusplus -} -#endif diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_helpers.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_helpers.h deleted file mode 100644 index 98250e1ac..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_helpers.h +++ /dev/null @@ -1,488 +0,0 @@ -#ifndef _MAVLINK_HELPERS_H_ -#define _MAVLINK_HELPERS_H_ - -#include "string.h" -#include "checksum.h" -#include "mavlink_types.h" - -#ifndef MAVLINK_HELPER -#define MAVLINK_HELPER -#endif - -/* - internal function to give access to the channel status for each channel - */ -MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) -{ - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_status[chan]; -} - -/** - * @brief Finalize a MAVLink message with channel assignment - * - * This function calculates the checksum and sets length and aircraft id correctly. - * It assumes that the message id and the payload are already correctly set. This function - * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack - * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. - * - * @param msg Message to finalize - * @param system_id Id of the sending (this) system, 1-127 - * @param length Message length - */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length, uint8_t crc_extra) -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length) -#endif -{ - // This code part is the same for all messages; - uint16_t checksum; - msg->magic = MAVLINK_STX; - msg->len = length; - msg->sysid = system_id; - msg->compid = component_id; - // One sequence number per component - msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN); -#if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); -#endif - mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); - mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); - - return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; -} - - -/** - * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel - */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length, uint8_t crc_extra) -{ - return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra); -} -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length) -{ - return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); - -/** - * @brief Finalize a MAVLink message with channel assignment and send - */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, - uint8_t length, uint8_t crc_extra) -#else -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length) -#endif -{ - uint16_t checksum; - uint8_t buf[MAVLINK_NUM_HEADER_BYTES]; - uint8_t ck[2]; - mavlink_status_t *status = mavlink_get_channel_status(chan); - buf[0] = MAVLINK_STX; - buf[1] = length; - buf[2] = status->current_tx_seq; - buf[3] = mavlink_system.sysid; - buf[4] = mavlink_system.compid; - buf[5] = msgid; - status->current_tx_seq++; - checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); - crc_accumulate_buffer(&checksum, packet, length); -#if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); -#endif - ck[0] = (uint8_t)(checksum & 0xFF); - ck[1] = (uint8_t)(checksum >> 8); - - MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); - _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES); - _mavlink_send_uart(chan, packet, length); - _mavlink_send_uart(chan, (const char *)ck, 2); - MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a message to send it over a serial byte stream - */ -MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) -{ - memcpy(buffer, (uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len); - return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; -} - -union __mavlink_bitfield { - uint8_t uint8; - int8_t int8; - uint16_t uint16; - int16_t int16; - uint32_t uint32; - int32_t int32; -}; - - -MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) -{ - crc_init(&msg->checksum); -} - -MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) -{ - crc_accumulate(c, &msg->checksum); -} - -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. Checksum and other failures will be silently - * ignored. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to barse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @return 0 if no message could be decoded, 1 else - * - * A typical use scenario of this function call is: - * - * @code - * #include // For fixed-width uint8_t type - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ -MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -{ - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; - - /* - default message crc function. You can override this per-system to - put this data in a different memory segment - */ -#if MAVLINK_CRC_EXTRA -#ifndef MAVLINK_MESSAGE_CRC - static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; -#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid] -#endif -#endif - - mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message - mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status - int bufferIndex = 0; - - status->msg_received = 0; - - switch (status->parse_state) - { - case MAVLINK_PARSE_STATE_UNINIT: - case MAVLINK_PARSE_STATE_IDLE: - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - mavlink_start_checksum(rxmsg); - } - break; - - case MAVLINK_PARSE_STATE_GOT_STX: - if (status->msg_received - /* Support shorter buffers than the - default maximum packet size */ -#if (MAVLINK_MAX_PAYLOAD_LEN < 255) - || c > MAVLINK_MAX_PAYLOAD_LEN -#endif - ) - { - status->buffer_overrun++; - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - } - else - { - // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 - rxmsg->len = c; - status->packet_idx = 0; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; - } - break; - - case MAVLINK_PARSE_STATE_GOT_LENGTH: - rxmsg->seq = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; - break; - - case MAVLINK_PARSE_STATE_GOT_SEQ: - rxmsg->sysid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; - break; - - case MAVLINK_PARSE_STATE_GOT_SYSID: - rxmsg->compid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; - break; - - case MAVLINK_PARSE_STATE_GOT_COMPID: - rxmsg->msgid = c; - mavlink_update_checksum(rxmsg, c); - if (rxmsg->len == 0) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; - } - break; - - case MAVLINK_PARSE_STATE_GOT_MSGID: - _MAV_PAYLOAD(rxmsg)[status->packet_idx++] = (char)c; - mavlink_update_checksum(rxmsg, c); - if (status->packet_idx == rxmsg->len) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - break; - - case MAVLINK_PARSE_STATE_GOT_PAYLOAD: -#if MAVLINK_CRC_EXTRA - mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid)); -#endif - if (c != (rxmsg->checksum & 0xFF)) { - // Check first checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - mavlink_start_checksum(rxmsg); - } - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; - } - break; - - case MAVLINK_PARSE_STATE_GOT_CRC1: - if (c != (rxmsg->checksum >> 8)) { - // Check second checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - mavlink_start_checksum(rxmsg); - } - } - else - { - // Successfully got message - status->msg_received = 1; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); - } - break; - } - - bufferIndex++; - // If a message has been sucessfully decoded, check index - if (status->msg_received == 1) - { - //while(status->current_seq != rxmsg->seq) - //{ - // status->packet_rx_drop_count++; - // status->current_seq++; - //} - status->current_rx_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; - } - - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; - r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; - r_mavlink_status->packet_rx_drop_count = status->parse_error; - status->parse_error = 0; - return status->msg_received; -} - -/** - * @brief Put a bitfield of length 1-32 bit into the buffer - * - * @param b the value to add, will be encoded in the bitfield - * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. - * @param packet_index the position in the packet (the index of the first byte to use) - * @param bit_index the position in the byte (the index of the first bit to use) - * @param buffer packet buffer to write into - * @return new position of the last used byte in the buffer - */ -MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) -{ - uint16_t bits_remain = bits; - // Transform number into network order - int32_t v; - uint8_t i_bit_index, i_byte_index, curr_bits_n; -#if MAVLINK_NEED_BYTE_SWAP - union { - int32_t i; - uint8_t b[4]; - } bin, bout; - bin.i = b; - bout.b[0] = bin.b[3]; - bout.b[1] = bin.b[2]; - bout.b[2] = bin.b[1]; - bout.b[3] = bin.b[0]; - v = bout.i; -#else - v = b; -#endif - - // buffer in - // 01100000 01000000 00000000 11110001 - // buffer out - // 11110001 00000000 01000000 01100000 - - // Existing partly filled byte (four free slots) - // 0111xxxx - - // Mask n free bits - // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 - // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 - - // Shift n bits into the right position - // out = in >> n; - - // Mask and shift bytes - i_bit_index = bit_index; - i_byte_index = packet_index; - if (bit_index > 0) - { - // If bits were available at start, they were available - // in the byte before the current index - i_byte_index--; - } - - // While bits have not been packed yet - while (bits_remain > 0) - { - // Bits still have to be packed - // there can be more than 8 bits, so - // we might have to pack them into more than one byte - - // First pack everything we can into the current 'open' byte - //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 - //FIXME - if (bits_remain <= (uint8_t)(8 - i_bit_index)) - { - // Enough space - curr_bits_n = (uint8_t)bits_remain; - } - else - { - curr_bits_n = (8 - i_bit_index); - } - - // Pack these n bits into the current byte - // Mask out whatever was at that position with ones (xxx11111) - buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); - // Put content to this position, by masking out the non-used part - buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v); - - // Increment the bit index - i_bit_index += curr_bits_n; - - // Now proceed to the next byte, if necessary - bits_remain -= curr_bits_n; - if (bits_remain > 0) - { - // Offer another 8 bits / one byte - i_byte_index++; - i_bit_index = 0; - } - } - - *r_bit_index = i_bit_index; - // If a partly filled byte is present, mark this as consumed - if (i_bit_index != 7) i_byte_index++; - return i_byte_index - packet_index; -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -// To make MAVLink work on your MCU, define comm_send_ch() if you wish -// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a -// whole packet at a time - -/* - -#include "mavlink_types.h" - -void comm_send_ch(mavlink_channel_t chan, uint8_t ch) -{ - if (chan == MAVLINK_COMM_0) - { - uart0_transmit(ch); - } - if (chan == MAVLINK_COMM_1) - { - uart1_transmit(ch); - } -} - */ - -MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len) -{ -#ifdef MAVLINK_SEND_UART_BYTES - /* this is the more efficient approach, if the platform - defines it */ - MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)buf, len); -#else - /* fallback to one byte at a time */ - uint16_t i; - for (i = 0; i < len; i++) { - comm_send_ch(chan, (uint8_t)buf[i]); - } -#endif -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - -#endif /* _MAVLINK_HELPERS_H_ */ diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_types.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_types.h deleted file mode 100644 index 630cb84b7..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_types.h +++ /dev/null @@ -1,300 +0,0 @@ -#ifndef MAVLINK_TYPES_H_ -#define MAVLINK_TYPES_H_ - -#include "inttypes.h" - -enum MAV_CLASS -{ - MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything - MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch - MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu - MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com - MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org - MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints - MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands - MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set - MAV_CLASS_NONE = 8, ///< No valid autopilot - MAV_CLASS_NB ///< Number of autopilot classes -}; - -enum MAV_ACTION -{ - MAV_ACTION_HOLD = 0, - MAV_ACTION_MOTORS_START = 1, - MAV_ACTION_LAUNCH = 2, - MAV_ACTION_RETURN = 3, - MAV_ACTION_EMCY_LAND = 4, - MAV_ACTION_EMCY_KILL = 5, - MAV_ACTION_CONFIRM_KILL = 6, - MAV_ACTION_CONTINUE = 7, - MAV_ACTION_MOTORS_STOP = 8, - MAV_ACTION_HALT = 9, - MAV_ACTION_SHUTDOWN = 10, - MAV_ACTION_REBOOT = 11, - MAV_ACTION_SET_MANUAL = 12, - MAV_ACTION_SET_AUTO = 13, - MAV_ACTION_STORAGE_READ = 14, - MAV_ACTION_STORAGE_WRITE = 15, - MAV_ACTION_CALIBRATE_RC = 16, - MAV_ACTION_CALIBRATE_GYRO = 17, - MAV_ACTION_CALIBRATE_MAG = 18, - MAV_ACTION_CALIBRATE_ACC = 19, - MAV_ACTION_CALIBRATE_PRESSURE = 20, - MAV_ACTION_REC_START = 21, - MAV_ACTION_REC_PAUSE = 22, - MAV_ACTION_REC_STOP = 23, - MAV_ACTION_TAKEOFF = 24, - MAV_ACTION_NAVIGATE = 25, - MAV_ACTION_LAND = 26, - MAV_ACTION_LOITER = 27, - MAV_ACTION_SET_ORIGIN = 28, - MAV_ACTION_RELAY_ON = 29, - MAV_ACTION_RELAY_OFF = 30, - MAV_ACTION_GET_IMAGE = 31, - MAV_ACTION_VIDEO_START = 32, - MAV_ACTION_VIDEO_STOP = 33, - MAV_ACTION_RESET_MAP = 34, - MAV_ACTION_RESET_PLAN = 35, - MAV_ACTION_DELAY_BEFORE_COMMAND = 36, - MAV_ACTION_ASCEND_AT_RATE = 37, - MAV_ACTION_CHANGE_MODE = 38, - MAV_ACTION_LOITER_MAX_TURNS = 39, - MAV_ACTION_LOITER_MAX_TIME = 40, - MAV_ACTION_START_HILSIM = 41, - MAV_ACTION_STOP_HILSIM = 42, - MAV_ACTION_NB ///< Number of MAV actions -}; - -enum MAV_MODE -{ - MAV_MODE_UNINIT = 0, ///< System is in undefined state - MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe - MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control - MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint - MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation - MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use - MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use - MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use - MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive - MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back -}; - -enum MAV_STATE -{ - MAV_STATE_UNINIT = 0, - MAV_STATE_BOOT, - MAV_STATE_CALIBRATING, - MAV_STATE_STANDBY, - MAV_STATE_ACTIVE, - MAV_STATE_CRITICAL, - MAV_STATE_EMERGENCY, - MAV_STATE_HILSIM, - MAV_STATE_POWEROFF -}; - -enum MAV_NAV -{ - MAV_NAV_GROUNDED = 0, - MAV_NAV_LIFTOFF, - MAV_NAV_HOLD, - MAV_NAV_WAYPOINT, - MAV_NAV_VECTOR, - MAV_NAV_RETURNING, - MAV_NAV_LANDING, - MAV_NAV_LOST, - MAV_NAV_LOITER, - MAV_NAV_FREE_DRIFT -}; - -enum MAV_TYPE -{ - MAV_GENERIC = 0, - MAV_FIXED_WING = 1, - MAV_QUADROTOR = 2, - MAV_COAXIAL = 3, - MAV_HELICOPTER = 4, - MAV_GROUND = 5, - OCU = 6, - MAV_AIRSHIP = 7, - MAV_FREE_BALLOON = 8, - MAV_ROCKET = 9, - UGV_GROUND_ROVER = 10, - UGV_SURFACE_SHIP = 11 -}; - -enum MAV_AUTOPILOT_TYPE -{ - MAV_AUTOPILOT_GENERIC = 0, - MAV_AUTOPILOT_PIXHAWK = 1, - MAV_AUTOPILOT_SLUGS = 2, - MAV_AUTOPILOT_ARDUPILOTMEGA = 3, - MAV_AUTOPILOT_NONE = 4 -}; - -enum MAV_COMPONENT -{ - MAV_COMP_ID_GPS, - MAV_COMP_ID_WAYPOINTPLANNER, - MAV_COMP_ID_BLOBTRACKER, - MAV_COMP_ID_PATHPLANNER, - MAV_COMP_ID_AIRSLAM, - MAV_COMP_ID_MAPPER, - MAV_COMP_ID_CAMERA, - MAV_COMP_ID_RADIO = 68, - MAV_COMP_ID_IMU = 200, - MAV_COMP_ID_IMU_2 = 201, - MAV_COMP_ID_IMU_3 = 202, - MAV_COMP_ID_UDP_BRIDGE = 240, - MAV_COMP_ID_UART_BRIDGE = 241, - MAV_COMP_ID_SYSTEM_CONTROL = 250 -}; - -enum MAV_FRAME -{ - MAV_FRAME_GLOBAL = 0, - MAV_FRAME_LOCAL = 1, - MAV_FRAME_MISSION = 2, - MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, - MAV_FRAME_LOCAL_ENU = 4 -}; - -enum MAVLINK_DATA_STREAM_TYPE -{ - MAVLINK_DATA_STREAM_IMG_JPEG, - MAVLINK_DATA_STREAM_IMG_BMP, - MAVLINK_DATA_STREAM_IMG_RAW8U, - MAVLINK_DATA_STREAM_IMG_RAW32U, - MAVLINK_DATA_STREAM_IMG_PGM, - MAVLINK_DATA_STREAM_IMG_PNG -}; - -#ifndef MAVLINK_MAX_PAYLOAD_LEN -// it is possible to override this, but be careful! -#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length -#endif - -#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) -#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum -#define MAVLINK_NUM_CHECKSUM_BYTES 2 -#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) - -#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length - -typedef struct param_union { - union { - float param_float; - int32_t param_int32; - uint32_t param_uint32; - }; - uint8_t type; -} mavlink_param_union_t; - -typedef struct __mavlink_system { - uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t type; ///< Unused, can be used by user to store the system's type - uint8_t state; ///< Unused, can be used by user to store the system's state - uint8_t mode; ///< Unused, can be used by user to store the system's mode - uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode -} mavlink_system_t; - -typedef struct __mavlink_message { - uint16_t checksum; /// sent at end of packet - uint8_t magic; ///< protocol magic marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload - uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; -} mavlink_message_t; - -typedef enum { - MAVLINK_TYPE_CHAR = 0, - MAVLINK_TYPE_UINT8_T = 1, - MAVLINK_TYPE_INT8_T = 2, - MAVLINK_TYPE_UINT16_T = 3, - MAVLINK_TYPE_INT16_T = 4, - MAVLINK_TYPE_UINT32_T = 5, - MAVLINK_TYPE_INT32_T = 6, - MAVLINK_TYPE_UINT64_T = 7, - MAVLINK_TYPE_INT64_T = 8, - MAVLINK_TYPE_FLOAT = 9, - MAVLINK_TYPE_DOUBLE = 10 -} mavlink_message_type_t; - -#define MAVLINK_MAX_FIELDS 64 - -typedef struct __mavlink_field_info { - const char *name; // name of this field - const char *print_format; // printing format hint, or NULL - mavlink_message_type_t type; // type of this field - unsigned array_length; // if non-zero, field is an array - unsigned wire_offset; // offset of each field in the payload - unsigned structure_offset; // offset in a C structure -} mavlink_field_info_t; - -// note that in this structure the order of fields is the order -// in the XML file, not necessary the wire order -typedef struct __mavlink_message_info { - const char *name; // name of the message - unsigned num_fields; // how many fields in this message - mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information -} mavlink_message_info_t; - -#define _MAV_PAYLOAD(msg) ((char *)(&(msg)->payload64[0])) -#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0]))) - -// checksum is immediately after the payload bytes -#define mavlink_ck_a(msg) *(msg->len + (uint8_t *)_MAV_PAYLOAD(msg)) -#define mavlink_ck_b(msg) *((msg->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD(msg)) - -typedef enum { - MAVLINK_COMM_0, - MAVLINK_COMM_1, - MAVLINK_COMM_2, - MAVLINK_COMM_3 -} mavlink_channel_t; - -/* - * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number - * of buffers they will use. If more are used, then the result will be - * a stack overrun - */ -#ifndef MAVLINK_COMM_NUM_BUFFERS -#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) -# define MAVLINK_COMM_NUM_BUFFERS 16 -#else -# define MAVLINK_COMM_NUM_BUFFERS 4 -#endif -#endif - -typedef enum { - MAVLINK_PARSE_STATE_UNINIT=0, - MAVLINK_PARSE_STATE_IDLE, - MAVLINK_PARSE_STATE_GOT_STX, - MAVLINK_PARSE_STATE_GOT_SEQ, - MAVLINK_PARSE_STATE_GOT_LENGTH, - MAVLINK_PARSE_STATE_GOT_SYSID, - MAVLINK_PARSE_STATE_GOT_COMPID, - MAVLINK_PARSE_STATE_GOT_MSGID, - MAVLINK_PARSE_STATE_GOT_PAYLOAD, - MAVLINK_PARSE_STATE_GOT_CRC1 -} mavlink_parse_state_t; ///< The state machine for the comm parser - -typedef struct __mavlink_status { - uint8_t msg_received; ///< Number of received messages - uint8_t buffer_overrun; ///< Number of buffer overruns - uint8_t parse_error; ///< Number of parse errors - mavlink_parse_state_t parse_state; ///< Parsing state machine - uint8_t packet_idx; ///< Index in current packet - uint8_t current_rx_seq; ///< Sequence number of last packet received - uint8_t current_tx_seq; ///< Sequence number of last packet sent - uint16_t packet_rx_success_count; ///< Received packets - uint16_t packet_rx_drop_count; ///< Number of packet drops -} mavlink_status_t; - -#define MAVLINK_BIG_ENDIAN 0 -#define MAVLINK_LITTLE_ENDIAN 1 - -#endif /* MAVLINK_TYPES_H_ */ diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/protocol.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/protocol.h deleted file mode 100644 index 05195c369..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/protocol.h +++ /dev/null @@ -1,319 +0,0 @@ -#ifndef _MAVLINK_PROTOCOL_H_ -#define _MAVLINK_PROTOCOL_H_ - -#include "string.h" -#include "mavlink_types.h" - -/* - If you want MAVLink on a system that is native big-endian, - you need to define NATIVE_BIG_ENDIAN -*/ -#ifdef NATIVE_BIG_ENDIAN -# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN) -#else -# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN) -#endif - -#ifndef MAVLINK_STACK_BUFFER -#define MAVLINK_STACK_BUFFER 0 -#endif - -#ifndef MAVLINK_AVOID_GCC_STACK_BUG -# define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__) -#endif - -#ifndef MAVLINK_ASSERT -#define MAVLINK_ASSERT(x) -#endif - -#ifndef MAVLINK_START_UART_SEND -#define MAVLINK_START_UART_SEND(chan, length) -#endif - -#ifndef MAVLINK_END_UART_SEND -#define MAVLINK_END_UART_SEND(chan, length) -#endif - -#ifdef MAVLINK_SEPARATE_HELPERS -#define MAVLINK_HELPER -#else -#define MAVLINK_HELPER static inline -#include "mavlink_helpers.h" -#endif // MAVLINK_SEPARATE_HELPERS - -/* always include the prototypes to ensure we don't get out of sync */ -MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan); -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length, uint8_t crc_extra); -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length, uint8_t crc_extra); -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, - uint8_t length, uint8_t crc_extra); -#endif -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length); -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length); -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length); -#endif // MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg); -MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg); -MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c); -MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status); -MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, - uint8_t* r_bit_index, uint8_t* buffer); -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); -#endif - -/** - * @brief Get the required buffer size for this message - */ -static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) -{ - return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; -} - -#if MAVLINK_NEED_BYTE_SWAP -static inline void byte_swap_2(char *dst, const char *src) -{ - dst[0] = src[1]; - dst[1] = src[0]; -} -static inline void byte_swap_4(char *dst, const char *src) -{ - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; -} -static inline void byte_swap_8(char *dst, const char *src) -{ - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; -} -#elif !MAVLINK_ALIGNED_FIELDS -static inline void byte_copy_2(char *dst, const char *src) -{ - dst[0] = src[0]; - dst[1] = src[1]; -} -static inline void byte_copy_4(char *dst, const char *src) -{ - dst[0] = src[0]; - dst[1] = src[1]; - dst[2] = src[2]; - dst[3] = src[3]; -} -static inline void byte_copy_8(char *dst, const char *src) -{ - memcpy(dst, src, 8); -} -#endif - -#define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b -#define _mav_put_int8_t(buf, wire_offset, b) buf[wire_offset] = (int8_t)b -#define _mav_put_char(buf, wire_offset, b) buf[wire_offset] = b - -#if MAVLINK_NEED_BYTE_SWAP -#define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b) -#define _mav_put_int16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b) -#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_int32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) -#define _mav_put_int64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) -#define _mav_put_float(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_double(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) -#elif !MAVLINK_ALIGNED_FIELDS -#define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b) -#define _mav_put_int16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b) -#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_int32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) -#define _mav_put_int64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) -#define _mav_put_float(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_double(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) -#else -#define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b -#define _mav_put_int16_t(buf, wire_offset, b) *(int16_t *)&buf[wire_offset] = b -#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b -#define _mav_put_int32_t(buf, wire_offset, b) *(int32_t *)&buf[wire_offset] = b -#define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b -#define _mav_put_int64_t(buf, wire_offset, b) *(int64_t *)&buf[wire_offset] = b -#define _mav_put_float(buf, wire_offset, b) *(float *)&buf[wire_offset] = b -#define _mav_put_double(buf, wire_offset, b) *(double *)&buf[wire_offset] = b -#endif - -/* - like memcpy(), but if src is NULL, do a memset to zero - */ -static void mav_array_memcpy(void *dest, const void *src, size_t n) -{ - if (src == NULL) { - memset(dest, 0, n); - } else { - memcpy(dest, src, n); - } -} - -/* - * Place a char array into a buffer - */ -static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length) -{ - mav_array_memcpy(&buf[wire_offset], b, array_length); -} - -/* - * Place a uint8_t array into a buffer - */ -static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length) -{ - mav_array_memcpy(&buf[wire_offset], b, array_length); -} - -/* - * Place a int8_t array into a buffer - */ -static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length) -{ - mav_array_memcpy(&buf[wire_offset], b, array_length); -} - -#if MAVLINK_NEED_BYTE_SWAP -#define _MAV_PUT_ARRAY(TYPE, V) \ -static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ -{ \ - if (b == NULL) { \ - memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \ - } else { \ - uint16_t i; \ - for (i=0; imsgid = MAVLINK_MSG_ID_TEST_TYPES; - return mavlink_finalize_message(msg, system_id, component_id, 179); -} - -/** - * @brief Pack a test_types message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param c char - * @param s string - * @param u8 uint8_t - * @param u16 uint16_t - * @param u32 uint32_t - * @param u64 uint64_t - * @param s8 int8_t - * @param s16 int16_t - * @param s32 int32_t - * @param s64 int64_t - * @param f float - * @param d double - * @param u8_array uint8_t_array - * @param u16_array uint16_t_array - * @param u32_array uint32_t_array - * @param u64_array uint64_t_array - * @param s8_array int8_t_array - * @param s16_array int16_t_array - * @param s32_array int32_t_array - * @param s64_array int64_t_array - * @param f_array float_array - * @param d_array double_array - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_test_types_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - char c,const char *s,uint8_t u8,uint16_t u16,uint32_t u32,uint64_t u64,int8_t s8,int16_t s16,int32_t s32,int64_t s64,float f,double d,const uint8_t *u8_array,const uint16_t *u16_array,const uint32_t *u32_array,const uint64_t *u64_array,const int8_t *s8_array,const int16_t *s16_array,const int32_t *s32_array,const int64_t *s64_array,const float *f_array,const double *d_array) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[179]; - _mav_put_char(buf, 0, c); - _mav_put_uint8_t(buf, 11, u8); - _mav_put_uint16_t(buf, 12, u16); - _mav_put_uint32_t(buf, 14, u32); - _mav_put_uint64_t(buf, 18, u64); - _mav_put_int8_t(buf, 26, s8); - _mav_put_int16_t(buf, 27, s16); - _mav_put_int32_t(buf, 29, s32); - _mav_put_int64_t(buf, 33, s64); - _mav_put_float(buf, 41, f); - _mav_put_double(buf, 45, d); - _mav_put_char_array(buf, 1, s, 10); - _mav_put_uint8_t_array(buf, 53, u8_array, 3); - _mav_put_uint16_t_array(buf, 56, u16_array, 3); - _mav_put_uint32_t_array(buf, 62, u32_array, 3); - _mav_put_uint64_t_array(buf, 74, u64_array, 3); - _mav_put_int8_t_array(buf, 98, s8_array, 3); - _mav_put_int16_t_array(buf, 101, s16_array, 3); - _mav_put_int32_t_array(buf, 107, s32_array, 3); - _mav_put_int64_t_array(buf, 119, s64_array, 3); - _mav_put_float_array(buf, 143, f_array, 3); - _mav_put_double_array(buf, 155, d_array, 3); - memcpy(_MAV_PAYLOAD(msg), buf, 179); -#else - mavlink_test_types_t packet; - packet.c = c; - packet.u8 = u8; - packet.u16 = u16; - packet.u32 = u32; - packet.u64 = u64; - packet.s8 = s8; - packet.s16 = s16; - packet.s32 = s32; - packet.s64 = s64; - packet.f = f; - packet.d = d; - mav_array_memcpy(packet.s, s, sizeof(char)*10); - mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3); - mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3); - mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3); - mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3); - mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3); - mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3); - mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3); - mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3); - mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3); - mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3); - memcpy(_MAV_PAYLOAD(msg), &packet, 179); -#endif - - msg->msgid = MAVLINK_MSG_ID_TEST_TYPES; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 179); -} - -/** - * @brief Encode a test_types struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param test_types C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_test_types_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_test_types_t* test_types) -{ - return mavlink_msg_test_types_pack(system_id, component_id, msg, test_types->c, test_types->s, test_types->u8, test_types->u16, test_types->u32, test_types->u64, test_types->s8, test_types->s16, test_types->s32, test_types->s64, test_types->f, test_types->d, test_types->u8_array, test_types->u16_array, test_types->u32_array, test_types->u64_array, test_types->s8_array, test_types->s16_array, test_types->s32_array, test_types->s64_array, test_types->f_array, test_types->d_array); -} - -/** - * @brief Send a test_types message - * @param chan MAVLink channel to send the message - * - * @param c char - * @param s string - * @param u8 uint8_t - * @param u16 uint16_t - * @param u32 uint32_t - * @param u64 uint64_t - * @param s8 int8_t - * @param s16 int16_t - * @param s32 int32_t - * @param s64 int64_t - * @param f float - * @param d double - * @param u8_array uint8_t_array - * @param u16_array uint16_t_array - * @param u32_array uint32_t_array - * @param u64_array uint64_t_array - * @param s8_array int8_t_array - * @param s16_array int16_t_array - * @param s32_array int32_t_array - * @param s64_array int64_t_array - * @param f_array float_array - * @param d_array double_array - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_test_types_send(mavlink_channel_t chan, char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[179]; - _mav_put_char(buf, 0, c); - _mav_put_uint8_t(buf, 11, u8); - _mav_put_uint16_t(buf, 12, u16); - _mav_put_uint32_t(buf, 14, u32); - _mav_put_uint64_t(buf, 18, u64); - _mav_put_int8_t(buf, 26, s8); - _mav_put_int16_t(buf, 27, s16); - _mav_put_int32_t(buf, 29, s32); - _mav_put_int64_t(buf, 33, s64); - _mav_put_float(buf, 41, f); - _mav_put_double(buf, 45, d); - _mav_put_char_array(buf, 1, s, 10); - _mav_put_uint8_t_array(buf, 53, u8_array, 3); - _mav_put_uint16_t_array(buf, 56, u16_array, 3); - _mav_put_uint32_t_array(buf, 62, u32_array, 3); - _mav_put_uint64_t_array(buf, 74, u64_array, 3); - _mav_put_int8_t_array(buf, 98, s8_array, 3); - _mav_put_int16_t_array(buf, 101, s16_array, 3); - _mav_put_int32_t_array(buf, 107, s32_array, 3); - _mav_put_int64_t_array(buf, 119, s64_array, 3); - _mav_put_float_array(buf, 143, f_array, 3); - _mav_put_double_array(buf, 155, d_array, 3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, buf, 179); -#else - mavlink_test_types_t packet; - packet.c = c; - packet.u8 = u8; - packet.u16 = u16; - packet.u32 = u32; - packet.u64 = u64; - packet.s8 = s8; - packet.s16 = s16; - packet.s32 = s32; - packet.s64 = s64; - packet.f = f; - packet.d = d; - mav_array_memcpy(packet.s, s, sizeof(char)*10); - mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3); - mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3); - mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3); - mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3); - mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3); - mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3); - mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3); - mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3); - mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3); - mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, (const char *)&packet, 179); -#endif -} - -#endif - -// MESSAGE TEST_TYPES UNPACKING - - -/** - * @brief Get field c from test_types message - * - * @return char - */ -static inline char mavlink_msg_test_types_get_c(const mavlink_message_t* msg) -{ - return _MAV_RETURN_char(msg, 0); -} - -/** - * @brief Get field s from test_types message - * - * @return string - */ -static inline uint16_t mavlink_msg_test_types_get_s(const mavlink_message_t* msg, char *s) -{ - return _MAV_RETURN_char_array(msg, s, 10, 1); -} - -/** - * @brief Get field u8 from test_types message - * - * @return uint8_t - */ -static inline uint8_t mavlink_msg_test_types_get_u8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 11); -} - -/** - * @brief Get field u16 from test_types message - * - * @return uint16_t - */ -static inline uint16_t mavlink_msg_test_types_get_u16(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field u32 from test_types message - * - * @return uint32_t - */ -static inline uint32_t mavlink_msg_test_types_get_u32(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 14); -} - -/** - * @brief Get field u64 from test_types message - * - * @return uint64_t - */ -static inline uint64_t mavlink_msg_test_types_get_u64(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 18); -} - -/** - * @brief Get field s8 from test_types message - * - * @return int8_t - */ -static inline int8_t mavlink_msg_test_types_get_s8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int8_t(msg, 26); -} - -/** - * @brief Get field s16 from test_types message - * - * @return int16_t - */ -static inline int16_t mavlink_msg_test_types_get_s16(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 27); -} - -/** - * @brief Get field s32 from test_types message - * - * @return int32_t - */ -static inline int32_t mavlink_msg_test_types_get_s32(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 29); -} - -/** - * @brief Get field s64 from test_types message - * - * @return int64_t - */ -static inline int64_t mavlink_msg_test_types_get_s64(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int64_t(msg, 33); -} - -/** - * @brief Get field f from test_types message - * - * @return float - */ -static inline float mavlink_msg_test_types_get_f(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 41); -} - -/** - * @brief Get field d from test_types message - * - * @return double - */ -static inline double mavlink_msg_test_types_get_d(const mavlink_message_t* msg) -{ - return _MAV_RETURN_double(msg, 45); -} - -/** - * @brief Get field u8_array from test_types message - * - * @return uint8_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_u8_array(const mavlink_message_t* msg, uint8_t *u8_array) -{ - return _MAV_RETURN_uint8_t_array(msg, u8_array, 3, 53); -} - -/** - * @brief Get field u16_array from test_types message - * - * @return uint16_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_u16_array(const mavlink_message_t* msg, uint16_t *u16_array) -{ - return _MAV_RETURN_uint16_t_array(msg, u16_array, 3, 56); -} - -/** - * @brief Get field u32_array from test_types message - * - * @return uint32_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_u32_array(const mavlink_message_t* msg, uint32_t *u32_array) -{ - return _MAV_RETURN_uint32_t_array(msg, u32_array, 3, 62); -} - -/** - * @brief Get field u64_array from test_types message - * - * @return uint64_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_u64_array(const mavlink_message_t* msg, uint64_t *u64_array) -{ - return _MAV_RETURN_uint64_t_array(msg, u64_array, 3, 74); -} - -/** - * @brief Get field s8_array from test_types message - * - * @return int8_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_s8_array(const mavlink_message_t* msg, int8_t *s8_array) -{ - return _MAV_RETURN_int8_t_array(msg, s8_array, 3, 98); -} - -/** - * @brief Get field s16_array from test_types message - * - * @return int16_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_s16_array(const mavlink_message_t* msg, int16_t *s16_array) -{ - return _MAV_RETURN_int16_t_array(msg, s16_array, 3, 101); -} - -/** - * @brief Get field s32_array from test_types message - * - * @return int32_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_s32_array(const mavlink_message_t* msg, int32_t *s32_array) -{ - return _MAV_RETURN_int32_t_array(msg, s32_array, 3, 107); -} - -/** - * @brief Get field s64_array from test_types message - * - * @return int64_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_s64_array(const mavlink_message_t* msg, int64_t *s64_array) -{ - return _MAV_RETURN_int64_t_array(msg, s64_array, 3, 119); -} - -/** - * @brief Get field f_array from test_types message - * - * @return float_array - */ -static inline uint16_t mavlink_msg_test_types_get_f_array(const mavlink_message_t* msg, float *f_array) -{ - return _MAV_RETURN_float_array(msg, f_array, 3, 143); -} - -/** - * @brief Get field d_array from test_types message - * - * @return double_array - */ -static inline uint16_t mavlink_msg_test_types_get_d_array(const mavlink_message_t* msg, double *d_array) -{ - return _MAV_RETURN_double_array(msg, d_array, 3, 155); -} - -/** - * @brief Decode a test_types message into a struct - * - * @param msg The message to decode - * @param test_types C-struct to decode the message contents into - */ -static inline void mavlink_msg_test_types_decode(const mavlink_message_t* msg, mavlink_test_types_t* test_types) -{ -#if MAVLINK_NEED_BYTE_SWAP - test_types->c = mavlink_msg_test_types_get_c(msg); - mavlink_msg_test_types_get_s(msg, test_types->s); - test_types->u8 = mavlink_msg_test_types_get_u8(msg); - test_types->u16 = mavlink_msg_test_types_get_u16(msg); - test_types->u32 = mavlink_msg_test_types_get_u32(msg); - test_types->u64 = mavlink_msg_test_types_get_u64(msg); - test_types->s8 = mavlink_msg_test_types_get_s8(msg); - test_types->s16 = mavlink_msg_test_types_get_s16(msg); - test_types->s32 = mavlink_msg_test_types_get_s32(msg); - test_types->s64 = mavlink_msg_test_types_get_s64(msg); - test_types->f = mavlink_msg_test_types_get_f(msg); - test_types->d = mavlink_msg_test_types_get_d(msg); - mavlink_msg_test_types_get_u8_array(msg, test_types->u8_array); - mavlink_msg_test_types_get_u16_array(msg, test_types->u16_array); - mavlink_msg_test_types_get_u32_array(msg, test_types->u32_array); - mavlink_msg_test_types_get_u64_array(msg, test_types->u64_array); - mavlink_msg_test_types_get_s8_array(msg, test_types->s8_array); - mavlink_msg_test_types_get_s16_array(msg, test_types->s16_array); - mavlink_msg_test_types_get_s32_array(msg, test_types->s32_array); - mavlink_msg_test_types_get_s64_array(msg, test_types->s64_array); - mavlink_msg_test_types_get_f_array(msg, test_types->f_array); - mavlink_msg_test_types_get_d_array(msg, test_types->d_array); -#else - memcpy(test_types, _MAV_PAYLOAD(msg), 179); -#endif -} diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/test.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/test.h deleted file mode 100644 index 23ee65986..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/test.h +++ /dev/null @@ -1,53 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from test.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef TEST_H -#define TEST_H - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {179, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {91, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_TEST_TYPES, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_TEST - - - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -// ENUM DEFINITIONS - - - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_test_types.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // TEST_H diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/testsuite.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/testsuite.h deleted file mode 100644 index 9b0fc041b..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/testsuite.h +++ /dev/null @@ -1,120 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from test.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef TEST_TESTSUITE_H -#define TEST_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL - -static void mavlink_test_test(uint8_t, uint8_t, mavlink_message_t *last_msg); - -static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - - mavlink_test_test(system_id, component_id, last_msg); -} -#endif - - - - -static void mavlink_test_test_types(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_test_types_t packet_in = { - 'A', - "BCDEFGHIJ", - 230, - 17859, - 963498192, - 93372036854776941ULL, - 211, - 18639, - 963498972, - 93372036854777886LL, - 304.0, - 438.0, - { 228, 229, 230 }, - { 20147, 20148, 20149 }, - { 963500688, 963500689, 963500690 }, - { 93372036854780469, 93372036854780470, 93372036854780471 }, - { 171, 172, 173 }, - { 22487, 22488, 22489 }, - { 963503028, 963503029, 963503030 }, - { 93372036854783304, 93372036854783305, 93372036854783306 }, - { 1018.0, 1019.0, 1020.0 }, - { 1208.0, 1209.0, 1210.0 }, - }; - mavlink_test_types_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.c = packet_in.c; - packet1.u8 = packet_in.u8; - packet1.u16 = packet_in.u16; - packet1.u32 = packet_in.u32; - packet1.u64 = packet_in.u64; - packet1.s8 = packet_in.s8; - packet1.s16 = packet_in.s16; - packet1.s32 = packet_in.s32; - packet1.s64 = packet_in.s64; - packet1.f = packet_in.f; - packet1.d = packet_in.d; - - mav_array_memcpy(packet1.s, packet_in.s, sizeof(char)*10); - mav_array_memcpy(packet1.u8_array, packet_in.u8_array, sizeof(uint8_t)*3); - mav_array_memcpy(packet1.u16_array, packet_in.u16_array, sizeof(uint16_t)*3); - mav_array_memcpy(packet1.u32_array, packet_in.u32_array, sizeof(uint32_t)*3); - mav_array_memcpy(packet1.u64_array, packet_in.u64_array, sizeof(uint64_t)*3); - mav_array_memcpy(packet1.s8_array, packet_in.s8_array, sizeof(int8_t)*3); - mav_array_memcpy(packet1.s16_array, packet_in.s16_array, sizeof(int16_t)*3); - mav_array_memcpy(packet1.s32_array, packet_in.s32_array, sizeof(int32_t)*3); - mav_array_memcpy(packet1.s64_array, packet_in.s64_array, sizeof(int64_t)*3); - mav_array_memcpy(packet1.f_array, packet_in.f_array, sizeof(float)*3); - mav_array_memcpy(packet1.d_array, packet_in.d_array, sizeof(double)*3); - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_test_types_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_test_types_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_test_types_pack(system_id, component_id, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); - mavlink_msg_test_types_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_test_types_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); - mavlink_msg_test_types_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; i>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); -} - -/** - * @brief Initiliaze the buffer for the X.25 CRC - * - * @param crcAccum the 16 bit X.25 CRC - */ -static inline void crc_init(uint16_t* crcAccum) -{ - *crcAccum = X25_INIT_CRC; -} - - -/** - * @brief Calculates the X.25 checksum on a byte buffer - * - * @param pBuffer buffer containing the byte array to hash - * @param length length of the byte array - * @return the checksum over the buffer bytes - **/ -static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) -{ - uint16_t crcTmp; - crc_init(&crcTmp); - while (length--) { - crc_accumulate(*pBuffer++, &crcTmp); - } - return crcTmp; -} - -/** - * @brief Accumulate the X.25 CRC by adding an array of bytes - * - * The checksum function adds the hash of one char at a time to the - * 16 bit checksum (uint16_t). - * - * @param data new bytes to hash - * @param crcAccum the already accumulated checksum - **/ -static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length) -{ - const uint8_t *p = (const uint8_t *)pBuffer; - while (length--) { - crc_accumulate(*p++, crcAccum); - } -} - - - - -#endif /* _CHECKSUM_H_ */ - -#ifdef __cplusplus -} -#endif diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h deleted file mode 100644 index 39d6930e5..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h +++ /dev/null @@ -1,507 +0,0 @@ -#ifndef _MAVLINK_HELPERS_H_ -#define _MAVLINK_HELPERS_H_ - -#include "string.h" -#include "checksum.h" -#include "mavlink_types.h" - -#ifndef MAVLINK_HELPER -#define MAVLINK_HELPER -#endif - -/* - internal function to give access to the channel status for each channel - */ -MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) -{ - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_status[chan]; -} - -/* - internal function to give access to the channel buffer for each channel - */ -MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) -{ - -#if MAVLINK_EXTERNAL_RX_BUFFER - // No m_mavlink_message array defined in function, - // has to be defined externally -#ifndef m_mavlink_message -#error ERROR: IF #define MAVLINK_EXTERNAL_RX_BUFFER IS SET, THE BUFFER HAS TO BE ALLOCATED OUTSIDE OF THIS FUNCTION (mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];) -#endif -#else - static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; -#endif - return &m_mavlink_buffer[chan]; -} - -/** - * @brief Finalize a MAVLink message with channel assignment - * - * This function calculates the checksum and sets length and aircraft id correctly. - * It assumes that the message id and the payload are already correctly set. This function - * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack - * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. - * - * @param msg Message to finalize - * @param system_id Id of the sending (this) system, 1-127 - * @param length Message length - */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length, uint8_t crc_extra) -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length) -#endif -{ - // This code part is the same for all messages; - uint16_t checksum; - msg->magic = MAVLINK_STX; - msg->len = length; - msg->sysid = system_id; - msg->compid = component_id; - // One sequence number per component - msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN); -#if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); -#endif - mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); - mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); - - return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; -} - - -/** - * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel - */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length, uint8_t crc_extra) -{ - return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra); -} -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length) -{ - return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); - -/** - * @brief Finalize a MAVLink message with channel assignment and send - */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, - uint8_t length, uint8_t crc_extra) -#else -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length) -#endif -{ - uint16_t checksum; - uint8_t buf[MAVLINK_NUM_HEADER_BYTES]; - uint8_t ck[2]; - mavlink_status_t *status = mavlink_get_channel_status(chan); - buf[0] = MAVLINK_STX; - buf[1] = length; - buf[2] = status->current_tx_seq; - buf[3] = mavlink_system.sysid; - buf[4] = mavlink_system.compid; - buf[5] = msgid; - status->current_tx_seq++; - checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); - crc_accumulate_buffer(&checksum, packet, length); -#if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); -#endif - ck[0] = (uint8_t)(checksum & 0xFF); - ck[1] = (uint8_t)(checksum >> 8); - - MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); - _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES); - _mavlink_send_uart(chan, packet, length); - _mavlink_send_uart(chan, (const char *)ck, 2); - MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a message to send it over a serial byte stream - */ -MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) -{ - memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len); - return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; -} - -union __mavlink_bitfield { - uint8_t uint8; - int8_t int8; - uint16_t uint16; - int16_t int16; - uint32_t uint32; - int32_t int32; -}; - - -MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) -{ - crc_init(&msg->checksum); -} - -MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) -{ - crc_accumulate(c, &msg->checksum); -} - -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. Checksum and other failures will be silently - * ignored. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to barse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @return 0 if no message could be decoded, 1 else - * - * A typical use scenario of this function call is: - * - * @code - * #include // For fixed-width uint8_t type - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ -MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -{ - /* - default message crc function. You can override this per-system to - put this data in a different memory segment - */ -#if MAVLINK_CRC_EXTRA -#ifndef MAVLINK_MESSAGE_CRC - static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; -#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid] -#endif -#endif - - mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); ///< The currently decoded message - mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status - int bufferIndex = 0; - - status->msg_received = 0; - - switch (status->parse_state) - { - case MAVLINK_PARSE_STATE_UNINIT: - case MAVLINK_PARSE_STATE_IDLE: - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - rxmsg->magic = c; - mavlink_start_checksum(rxmsg); - } - break; - - case MAVLINK_PARSE_STATE_GOT_STX: - if (status->msg_received -/* Support shorter buffers than the - default maximum packet size */ -#if (MAVLINK_MAX_PAYLOAD_LEN < 255) - || c > MAVLINK_MAX_PAYLOAD_LEN -#endif - ) - { - status->buffer_overrun++; - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - } - else - { - // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 - rxmsg->len = c; - status->packet_idx = 0; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; - } - break; - - case MAVLINK_PARSE_STATE_GOT_LENGTH: - rxmsg->seq = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; - break; - - case MAVLINK_PARSE_STATE_GOT_SEQ: - rxmsg->sysid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; - break; - - case MAVLINK_PARSE_STATE_GOT_SYSID: - rxmsg->compid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; - break; - - case MAVLINK_PARSE_STATE_GOT_COMPID: - rxmsg->msgid = c; - mavlink_update_checksum(rxmsg, c); - if (rxmsg->len == 0) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; - } - break; - - case MAVLINK_PARSE_STATE_GOT_MSGID: - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c; - mavlink_update_checksum(rxmsg, c); - if (status->packet_idx == rxmsg->len) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - break; - - case MAVLINK_PARSE_STATE_GOT_PAYLOAD: -#if MAVLINK_CRC_EXTRA - mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid)); -#endif - if (c != (rxmsg->checksum & 0xFF)) { - // Check first checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - mavlink_start_checksum(rxmsg); - } - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c; - } - break; - - case MAVLINK_PARSE_STATE_GOT_CRC1: - if (c != (rxmsg->checksum >> 8)) { - // Check second checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - mavlink_start_checksum(rxmsg); - } - } - else - { - // Successfully got message - status->msg_received = 1; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c; - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); - } - break; - } - - bufferIndex++; - // If a message has been sucessfully decoded, check index - if (status->msg_received == 1) - { - //while(status->current_seq != rxmsg->seq) - //{ - // status->packet_rx_drop_count++; - // status->current_seq++; - //} - status->current_rx_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; - } - - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; - r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; - r_mavlink_status->packet_rx_drop_count = status->parse_error; - status->parse_error = 0; - return status->msg_received; -} - -/** - * @brief Put a bitfield of length 1-32 bit into the buffer - * - * @param b the value to add, will be encoded in the bitfield - * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. - * @param packet_index the position in the packet (the index of the first byte to use) - * @param bit_index the position in the byte (the index of the first bit to use) - * @param buffer packet buffer to write into - * @return new position of the last used byte in the buffer - */ -MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) -{ - uint16_t bits_remain = bits; - // Transform number into network order - int32_t v; - uint8_t i_bit_index, i_byte_index, curr_bits_n; -#if MAVLINK_NEED_BYTE_SWAP - union { - int32_t i; - uint8_t b[4]; - } bin, bout; - bin.i = b; - bout.b[0] = bin.b[3]; - bout.b[1] = bin.b[2]; - bout.b[2] = bin.b[1]; - bout.b[3] = bin.b[0]; - v = bout.i; -#else - v = b; -#endif - - // buffer in - // 01100000 01000000 00000000 11110001 - // buffer out - // 11110001 00000000 01000000 01100000 - - // Existing partly filled byte (four free slots) - // 0111xxxx - - // Mask n free bits - // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 - // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 - - // Shift n bits into the right position - // out = in >> n; - - // Mask and shift bytes - i_bit_index = bit_index; - i_byte_index = packet_index; - if (bit_index > 0) - { - // If bits were available at start, they were available - // in the byte before the current index - i_byte_index--; - } - - // While bits have not been packed yet - while (bits_remain > 0) - { - // Bits still have to be packed - // there can be more than 8 bits, so - // we might have to pack them into more than one byte - - // First pack everything we can into the current 'open' byte - //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 - //FIXME - if (bits_remain <= (uint8_t)(8 - i_bit_index)) - { - // Enough space - curr_bits_n = (uint8_t)bits_remain; - } - else - { - curr_bits_n = (8 - i_bit_index); - } - - // Pack these n bits into the current byte - // Mask out whatever was at that position with ones (xxx11111) - buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); - // Put content to this position, by masking out the non-used part - buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v); - - // Increment the bit index - i_bit_index += curr_bits_n; - - // Now proceed to the next byte, if necessary - bits_remain -= curr_bits_n; - if (bits_remain > 0) - { - // Offer another 8 bits / one byte - i_byte_index++; - i_bit_index = 0; - } - } - - *r_bit_index = i_bit_index; - // If a partly filled byte is present, mark this as consumed - if (i_bit_index != 7) i_byte_index++; - return i_byte_index - packet_index; -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -// To make MAVLink work on your MCU, define comm_send_ch() if you wish -// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a -// whole packet at a time - -/* - -#include "mavlink_types.h" - -void comm_send_ch(mavlink_channel_t chan, uint8_t ch) -{ - if (chan == MAVLINK_COMM_0) - { - uart0_transmit(ch); - } - if (chan == MAVLINK_COMM_1) - { - uart1_transmit(ch); - } -} - */ - -MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len) -{ -#ifdef MAVLINK_SEND_UART_BYTES - /* this is the more efficient approach, if the platform - defines it */ - MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)buf, len); -#else - /* fallback to one byte at a time */ - uint16_t i; - for (i = 0; i < len; i++) { - comm_send_ch(chan, (uint8_t)buf[i]); - } -#endif -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - -#endif /* _MAVLINK_HELPERS_H_ */ diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_protobuf_manager.hpp b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_protobuf_manager.hpp deleted file mode 100644 index fd3ddd026..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_protobuf_manager.hpp +++ /dev/null @@ -1,377 +0,0 @@ -#ifndef MAVLINKPROTOBUFMANAGER_HPP -#define MAVLINKPROTOBUFMANAGER_HPP - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace mavlink -{ - -class ProtobufManager -{ -public: - ProtobufManager() - : mRegisteredTypeCount(0) - , mStreamID(0) - , mVerbose(false) - , kExtendedHeaderSize(MAVLINK_EXTENDED_HEADER_LEN) - , kExtendedPayloadMaxSize(MAVLINK_MAX_EXTENDED_PAYLOAD_LEN) - { - // register GLOverlay - { - std::tr1::shared_ptr msg(new px::GLOverlay); - registerType(msg); - } - - // register ObstacleList - { - std::tr1::shared_ptr msg(new px::ObstacleList); - registerType(msg); - } - - // register ObstacleMap - { - std::tr1::shared_ptr msg(new px::ObstacleMap); - registerType(msg); - } - - // register Path - { - std::tr1::shared_ptr msg(new px::Path); - registerType(msg); - } - - // register PointCloudXYZI - { - std::tr1::shared_ptr msg(new px::PointCloudXYZI); - registerType(msg); - } - - // register PointCloudXYZRGB - { - std::tr1::shared_ptr msg(new px::PointCloudXYZRGB); - registerType(msg); - } - - // register RGBDImage - { - std::tr1::shared_ptr msg(new px::RGBDImage); - registerType(msg); - } - - srand(time(NULL)); - mStreamID = rand() + 1; - } - - bool fragmentMessage(uint8_t system_id, uint8_t component_id, - uint8_t target_system, uint8_t target_component, - const google::protobuf::Message& protobuf_msg, - std::vector& fragments) const - { - TypeMap::const_iterator it = mTypeMap.find(protobuf_msg.GetTypeName()); - if (it == mTypeMap.end()) - { - std::cout << "# WARNING: Protobuf message with type " - << protobuf_msg.GetTypeName() << " is not registered." - << std::endl; - return false; - } - - uint8_t typecode = it->second; - - std::string data = protobuf_msg.SerializeAsString(); - - int fragmentCount = (protobuf_msg.ByteSize() + kExtendedPayloadMaxSize - 1) / kExtendedPayloadMaxSize; - unsigned int offset = 0; - - for (int i = 0; i < fragmentCount; ++i) - { - mavlink_extended_message_t fragment; - - // write extended header data - uint8_t* payload = reinterpret_cast(fragment.base_msg.payload64); - unsigned int length = 0; - uint8_t flags = 0; - - if (i < fragmentCount - 1) - { - length = kExtendedPayloadMaxSize; - flags |= 0x1; - } - else - { - length = protobuf_msg.ByteSize() - kExtendedPayloadMaxSize * (fragmentCount - 1); - } - - memcpy(payload, &target_system, 1); - memcpy(payload + 1, &target_component, 1); - memcpy(payload + 2, &typecode, 1); - memcpy(payload + 3, &length, 4); - memcpy(payload + 7, &mStreamID, 2); - memcpy(payload + 9, &offset, 4); - memcpy(payload + 13, &flags, 1); - - fragment.base_msg.msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE; - mavlink_finalize_message(&fragment.base_msg, system_id, component_id, kExtendedHeaderSize, 0); - - // write extended payload data - fragment.extended_payload_len = length; - memcpy(fragment.extended_payload, &data[offset], length); - - fragments.push_back(fragment); - offset += length; - } - - if (mVerbose) - { - std::cerr << "# INFO: Split extended message with size " - << protobuf_msg.ByteSize() << " into " - << fragmentCount << " fragments." << std::endl; - } - - return true; - } - - bool cacheFragment(mavlink_extended_message_t& msg) - { - if (!validFragment(msg)) - { - if (mVerbose) - { - std::cerr << "# WARNING: Message is not a valid fragment. " - << "Dropping message..." << std::endl; - } - return false; - } - - // read extended header - uint8_t* payload = reinterpret_cast(msg.base_msg.payload64); - uint8_t typecode = 0; - unsigned int length = 0; - unsigned short streamID = 0; - unsigned int offset = 0; - uint8_t flags = 0; - - memcpy(&typecode, payload + 2, 1); - memcpy(&length, payload + 3, 4); - memcpy(&streamID, payload + 7, 2); - memcpy(&offset, payload + 9, 4); - memcpy(&flags, payload + 13, 1); - - if (typecode >= mTypeMap.size()) - { - std::cout << "# WARNING: Protobuf message with type code " - << static_cast(typecode) << " is not registered." << std::endl; - return false; - } - - bool reassemble = false; - - FragmentQueue::iterator it = mFragmentQueue.find(streamID); - if (it == mFragmentQueue.end()) - { - if (offset == 0) - { - mFragmentQueue[streamID].push_back(msg); - - if ((flags & 0x1) != 0x1) - { - reassemble = true; - } - - if (mVerbose) - { - std::cerr << "# INFO: Added fragment to new queue." - << std::endl; - } - } - else - { - if (mVerbose) - { - std::cerr << "# WARNING: Message is not a valid fragment. " - << "Dropping message..." << std::endl; - } - } - } - else - { - std::deque& queue = it->second; - - if (queue.empty()) - { - if (offset == 0) - { - queue.push_back(msg); - - if ((flags & 0x1) != 0x1) - { - reassemble = true; - } - } - else - { - if (mVerbose) - { - std::cerr << "# WARNING: Message is not a valid fragment. " - << "Dropping message..." << std::endl; - } - } - } - else - { - if (fragmentDataSize(queue.back()) + fragmentOffset(queue.back()) != offset) - { - if (mVerbose) - { - std::cerr << "# WARNING: Previous fragment(s) have been lost. " - << "Dropping message and clearing queue..." << std::endl; - } - queue.clear(); - } - else - { - queue.push_back(msg); - - if ((flags & 0x1) != 0x1) - { - reassemble = true; - } - } - } - } - - if (reassemble) - { - std::deque& queue = mFragmentQueue[streamID]; - - std::string data; - for (size_t i = 0; i < queue.size(); ++i) - { - mavlink_extended_message_t& mavlink_msg = queue.at(i); - - data.append(reinterpret_cast(&mavlink_msg.extended_payload[0]), - static_cast(mavlink_msg.extended_payload_len)); - } - - mMessages.at(typecode)->ParseFromString(data); - - mMessageAvailable.at(typecode) = true; - - queue.clear(); - - if (mVerbose) - { - std::cerr << "# INFO: Reassembled fragments for message with typename " - << mMessages.at(typecode)->GetTypeName() << " and size " - << mMessages.at(typecode)->ByteSize() - << "." << std::endl; - } - } - - return true; - } - - bool getMessage(std::tr1::shared_ptr& msg) - { - for (size_t i = 0; i < mMessageAvailable.size(); ++i) - { - if (mMessageAvailable.at(i)) - { - msg = mMessages.at(i); - mMessageAvailable.at(i) = false; - - return true; - } - } - - return false; - } - -private: - void registerType(const std::tr1::shared_ptr& msg) - { - mTypeMap[msg->GetTypeName()] = mRegisteredTypeCount; - ++mRegisteredTypeCount; - mMessages.push_back(msg); - mMessageAvailable.push_back(false); - } - - bool validFragment(const mavlink_extended_message_t& msg) const - { - if (msg.base_msg.magic != MAVLINK_STX || - msg.base_msg.len != kExtendedHeaderSize || - msg.base_msg.msgid != MAVLINK_MSG_ID_EXTENDED_MESSAGE) - { - return false; - } - - uint16_t checksum; - checksum = crc_calculate(reinterpret_cast(&msg.base_msg.len), MAVLINK_CORE_HEADER_LEN); - crc_accumulate_buffer(&checksum, reinterpret_cast(&msg.base_msg.payload64), kExtendedHeaderSize); -#if MAVLINK_CRC_EXTRA - static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; - crc_accumulate(mavlink_message_crcs[msg.base_msg.msgid], &checksum); -#endif - - if (mavlink_ck_a(&(msg.base_msg)) != (uint8_t)(checksum & 0xFF) && - mavlink_ck_b(&(msg.base_msg)) != (uint8_t)(checksum >> 8)) - { - return false; - } - - return true; - } - - unsigned int fragmentDataSize(const mavlink_extended_message_t& msg) const - { - const uint8_t* payload = reinterpret_cast(msg.base_msg.payload64); - - return *(reinterpret_cast(payload + 3)); - } - - unsigned int fragmentOffset(const mavlink_extended_message_t& msg) const - { - const uint8_t* payload = reinterpret_cast(msg.base_msg.payload64); - - return *(reinterpret_cast(payload + 9)); - } - - int mRegisteredTypeCount; - unsigned short mStreamID; - bool mVerbose; - - typedef std::map TypeMap; - TypeMap mTypeMap; - std::vector< std::tr1::shared_ptr > mMessages; - std::vector mMessageAvailable; - - typedef std::map > FragmentQueue; - FragmentQueue mFragmentQueue; - - const int kExtendedHeaderSize; - /** - * Extended header structure - * ========================= - * byte 0 - target_system - * byte 1 - target_component - * byte 2 - extended message id (type code) - * bytes 3-6 - extended payload size in bytes - * byte 7-8 - stream ID - * byte 9-12 - fragment offset - * byte 13 - fragment flags (bit 0 - 1=more fragments, 0=last fragment) - */ - - const int kExtendedPayloadMaxSize; -}; - -} - -#endif diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_types.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_types.h deleted file mode 100644 index 5fbde97f7..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_types.h +++ /dev/null @@ -1,158 +0,0 @@ -#ifndef MAVLINK_TYPES_H_ -#define MAVLINK_TYPES_H_ - -#include - -#ifndef MAVLINK_MAX_PAYLOAD_LEN -// it is possible to override this, but be careful! -#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length -#endif - -#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) -#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum -#define MAVLINK_NUM_CHECKSUM_BYTES 2 -#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) - -#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length - -#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255 -#define MAVLINK_EXTENDED_HEADER_LEN 14 - -#if (defined _MSC_VER) | ((defined __APPLE__) & (defined __MACH__)) | (defined __linux__) - /* full fledged 32bit++ OS */ - #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507 -#else - /* small microcontrollers */ - #define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048 -#endif - -#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) - -typedef struct param_union { - union { - float param_float; - int32_t param_int32; - uint32_t param_uint32; - uint8_t param_uint8; - uint8_t bytes[4]; - }; - uint8_t type; -} mavlink_param_union_t; - -typedef struct __mavlink_system { - uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t type; ///< Unused, can be used by user to store the system's type - uint8_t state; ///< Unused, can be used by user to store the system's state - uint8_t mode; ///< Unused, can be used by user to store the system's mode - uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode -} mavlink_system_t; - -typedef struct __mavlink_message { - uint16_t checksum; /// sent at end of packet - uint8_t magic; ///< protocol magic marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload - uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; -} mavlink_message_t; - - -typedef struct __mavlink_extended_message { - mavlink_message_t base_msg; - int32_t extended_payload_len; ///< Length of extended payload if any - uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; -} mavlink_extended_message_t; - - -typedef enum { - MAVLINK_TYPE_CHAR = 0, - MAVLINK_TYPE_UINT8_T = 1, - MAVLINK_TYPE_INT8_T = 2, - MAVLINK_TYPE_UINT16_T = 3, - MAVLINK_TYPE_INT16_T = 4, - MAVLINK_TYPE_UINT32_T = 5, - MAVLINK_TYPE_INT32_T = 6, - MAVLINK_TYPE_UINT64_T = 7, - MAVLINK_TYPE_INT64_T = 8, - MAVLINK_TYPE_FLOAT = 9, - MAVLINK_TYPE_DOUBLE = 10 -} mavlink_message_type_t; - -#define MAVLINK_MAX_FIELDS 64 - -typedef struct __mavlink_field_info { - const char *name; // name of this field - const char *print_format; // printing format hint, or NULL - mavlink_message_type_t type; // type of this field - unsigned int array_length; // if non-zero, field is an array - unsigned int wire_offset; // offset of each field in the payload - unsigned int structure_offset; // offset in a C structure -} mavlink_field_info_t; - -// note that in this structure the order of fields is the order -// in the XML file, not necessary the wire order -typedef struct __mavlink_message_info { - const char *name; // name of the message - unsigned num_fields; // how many fields in this message - mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information -} mavlink_message_info_t; - -#define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0]))) -#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0]))) - -// checksum is immediately after the payload bytes -#define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) -#define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) - -typedef enum { - MAVLINK_COMM_0, - MAVLINK_COMM_1, - MAVLINK_COMM_2, - MAVLINK_COMM_3 -} mavlink_channel_t; - -/* - * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number - * of buffers they will use. If more are used, then the result will be - * a stack overrun - */ -#ifndef MAVLINK_COMM_NUM_BUFFERS -#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) -# define MAVLINK_COMM_NUM_BUFFERS 16 -#else -# define MAVLINK_COMM_NUM_BUFFERS 4 -#endif -#endif - -typedef enum { - MAVLINK_PARSE_STATE_UNINIT=0, - MAVLINK_PARSE_STATE_IDLE, - MAVLINK_PARSE_STATE_GOT_STX, - MAVLINK_PARSE_STATE_GOT_SEQ, - MAVLINK_PARSE_STATE_GOT_LENGTH, - MAVLINK_PARSE_STATE_GOT_SYSID, - MAVLINK_PARSE_STATE_GOT_COMPID, - MAVLINK_PARSE_STATE_GOT_MSGID, - MAVLINK_PARSE_STATE_GOT_PAYLOAD, - MAVLINK_PARSE_STATE_GOT_CRC1 -} mavlink_parse_state_t; ///< The state machine for the comm parser - -typedef struct __mavlink_status { - uint8_t msg_received; ///< Number of received messages - uint8_t buffer_overrun; ///< Number of buffer overruns - uint8_t parse_error; ///< Number of parse errors - mavlink_parse_state_t parse_state; ///< Parsing state machine - uint8_t packet_idx; ///< Index in current packet - uint8_t current_rx_seq; ///< Sequence number of last packet received - uint8_t current_tx_seq; ///< Sequence number of last packet sent - uint16_t packet_rx_success_count; ///< Received packets - uint16_t packet_rx_drop_count; ///< Number of packet drops -} mavlink_status_t; - -#define MAVLINK_BIG_ENDIAN 0 -#define MAVLINK_LITTLE_ENDIAN 1 - -#endif /* MAVLINK_TYPES_H_ */ diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/pixhawk/pixhawk.pb.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/pixhawk/pixhawk.pb.h deleted file mode 100644 index 7556606e9..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/pixhawk/pixhawk.pb.h +++ /dev/null @@ -1,3663 +0,0 @@ -// Generated by the protocol buffer compiler. DO NOT EDIT! -// source: pixhawk.proto - -#ifndef PROTOBUF_pixhawk_2eproto__INCLUDED -#define PROTOBUF_pixhawk_2eproto__INCLUDED - -#include - -#include - -#if GOOGLE_PROTOBUF_VERSION < 2004000 -#error This file was generated by a newer version of protoc which is -#error incompatible with your Protocol Buffer headers. Please update -#error your headers. -#endif -#if 2004001 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION -#error This file was generated by an older version of protoc which is -#error incompatible with your Protocol Buffer headers. Please -#error regenerate this file with a newer version of protoc. -#endif - -#include -#include -#include -#include -// @@protoc_insertion_point(includes) - -namespace px { - -// Internal implementation detail -- do not call these. -void protobuf_AddDesc_pixhawk_2eproto(); -void protobuf_AssignDesc_pixhawk_2eproto(); -void protobuf_ShutdownFile_pixhawk_2eproto(); - -class HeaderInfo; -class GLOverlay; -class Obstacle; -class ObstacleList; -class ObstacleMap; -class Path; -class PointCloudXYZI; -class PointCloudXYZI_PointXYZI; -class PointCloudXYZRGB; -class PointCloudXYZRGB_PointXYZRGB; -class RGBDImage; -class Waypoint; - -enum GLOverlay_CoordinateFrameType { - GLOverlay_CoordinateFrameType_GLOBAL = 0, - GLOverlay_CoordinateFrameType_LOCAL = 1 -}; -bool GLOverlay_CoordinateFrameType_IsValid(int value); -const GLOverlay_CoordinateFrameType GLOverlay_CoordinateFrameType_CoordinateFrameType_MIN = GLOverlay_CoordinateFrameType_GLOBAL; -const GLOverlay_CoordinateFrameType GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX = GLOverlay_CoordinateFrameType_LOCAL; -const int GLOverlay_CoordinateFrameType_CoordinateFrameType_ARRAYSIZE = GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX + 1; - -const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor(); -inline const ::std::string& GLOverlay_CoordinateFrameType_Name(GLOverlay_CoordinateFrameType value) { - return ::google::protobuf::internal::NameOfEnum( - GLOverlay_CoordinateFrameType_descriptor(), value); -} -inline bool GLOverlay_CoordinateFrameType_Parse( - const ::std::string& name, GLOverlay_CoordinateFrameType* value) { - return ::google::protobuf::internal::ParseNamedEnum( - GLOverlay_CoordinateFrameType_descriptor(), name, value); -} -enum GLOverlay_Mode { - GLOverlay_Mode_POINTS = 0, - GLOverlay_Mode_LINES = 1, - GLOverlay_Mode_LINE_STRIP = 2, - GLOverlay_Mode_LINE_LOOP = 3, - GLOverlay_Mode_TRIANGLES = 4, - GLOverlay_Mode_TRIANGLE_STRIP = 5, - GLOverlay_Mode_TRIANGLE_FAN = 6, - GLOverlay_Mode_QUADS = 7, - GLOverlay_Mode_QUAD_STRIP = 8, - GLOverlay_Mode_POLYGON = 9, - GLOverlay_Mode_SOLID_CIRCLE = 10, - GLOverlay_Mode_WIRE_CIRCLE = 11, - GLOverlay_Mode_SOLID_CUBE = 12, - GLOverlay_Mode_WIRE_CUBE = 13 -}; -bool GLOverlay_Mode_IsValid(int value); -const GLOverlay_Mode GLOverlay_Mode_Mode_MIN = GLOverlay_Mode_POINTS; -const GLOverlay_Mode GLOverlay_Mode_Mode_MAX = GLOverlay_Mode_WIRE_CUBE; -const int GLOverlay_Mode_Mode_ARRAYSIZE = GLOverlay_Mode_Mode_MAX + 1; - -const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor(); -inline const ::std::string& GLOverlay_Mode_Name(GLOverlay_Mode value) { - return ::google::protobuf::internal::NameOfEnum( - GLOverlay_Mode_descriptor(), value); -} -inline bool GLOverlay_Mode_Parse( - const ::std::string& name, GLOverlay_Mode* value) { - return ::google::protobuf::internal::ParseNamedEnum( - GLOverlay_Mode_descriptor(), name, value); -} -enum GLOverlay_Identifier { - GLOverlay_Identifier_END = 14, - GLOverlay_Identifier_VERTEX2F = 15, - GLOverlay_Identifier_VERTEX3F = 16, - GLOverlay_Identifier_ROTATEF = 17, - GLOverlay_Identifier_TRANSLATEF = 18, - GLOverlay_Identifier_SCALEF = 19, - GLOverlay_Identifier_PUSH_MATRIX = 20, - GLOverlay_Identifier_POP_MATRIX = 21, - GLOverlay_Identifier_COLOR3F = 22, - GLOverlay_Identifier_COLOR4F = 23, - GLOverlay_Identifier_POINTSIZE = 24, - GLOverlay_Identifier_LINEWIDTH = 25 -}; -bool GLOverlay_Identifier_IsValid(int value); -const GLOverlay_Identifier GLOverlay_Identifier_Identifier_MIN = GLOverlay_Identifier_END; -const GLOverlay_Identifier GLOverlay_Identifier_Identifier_MAX = GLOverlay_Identifier_LINEWIDTH; -const int GLOverlay_Identifier_Identifier_ARRAYSIZE = GLOverlay_Identifier_Identifier_MAX + 1; - -const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor(); -inline const ::std::string& GLOverlay_Identifier_Name(GLOverlay_Identifier value) { - return ::google::protobuf::internal::NameOfEnum( - GLOverlay_Identifier_descriptor(), value); -} -inline bool GLOverlay_Identifier_Parse( - const ::std::string& name, GLOverlay_Identifier* value) { - return ::google::protobuf::internal::ParseNamedEnum( - GLOverlay_Identifier_descriptor(), name, value); -} -// =================================================================== - -class HeaderInfo : public ::google::protobuf::Message { - public: - HeaderInfo(); - virtual ~HeaderInfo(); - - HeaderInfo(const HeaderInfo& from); - - inline HeaderInfo& operator=(const HeaderInfo& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const HeaderInfo& default_instance(); - - void Swap(HeaderInfo* other); - - // implements Message ---------------------------------------------- - - HeaderInfo* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const HeaderInfo& from); - void MergeFrom(const HeaderInfo& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required int32 source_sysid = 1; - inline bool has_source_sysid() const; - inline void clear_source_sysid(); - static const int kSourceSysidFieldNumber = 1; - inline ::google::protobuf::int32 source_sysid() const; - inline void set_source_sysid(::google::protobuf::int32 value); - - // required int32 source_compid = 2; - inline bool has_source_compid() const; - inline void clear_source_compid(); - static const int kSourceCompidFieldNumber = 2; - inline ::google::protobuf::int32 source_compid() const; - inline void set_source_compid(::google::protobuf::int32 value); - - // required double timestamp = 3; - inline bool has_timestamp() const; - inline void clear_timestamp(); - static const int kTimestampFieldNumber = 3; - inline double timestamp() const; - inline void set_timestamp(double value); - - // @@protoc_insertion_point(class_scope:px.HeaderInfo) - private: - inline void set_has_source_sysid(); - inline void clear_has_source_sysid(); - inline void set_has_source_compid(); - inline void clear_has_source_compid(); - inline void set_has_timestamp(); - inline void clear_has_timestamp(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::google::protobuf::int32 source_sysid_; - ::google::protobuf::int32 source_compid_; - double timestamp_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(3 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static HeaderInfo* default_instance_; -}; -// ------------------------------------------------------------------- - -class GLOverlay : public ::google::protobuf::Message { - public: - GLOverlay(); - virtual ~GLOverlay(); - - GLOverlay(const GLOverlay& from); - - inline GLOverlay& operator=(const GLOverlay& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const GLOverlay& default_instance(); - - void Swap(GLOverlay* other); - - // implements Message ---------------------------------------------- - - GLOverlay* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const GLOverlay& from); - void MergeFrom(const GLOverlay& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - typedef GLOverlay_CoordinateFrameType CoordinateFrameType; - static const CoordinateFrameType GLOBAL = GLOverlay_CoordinateFrameType_GLOBAL; - static const CoordinateFrameType LOCAL = GLOverlay_CoordinateFrameType_LOCAL; - static inline bool CoordinateFrameType_IsValid(int value) { - return GLOverlay_CoordinateFrameType_IsValid(value); - } - static const CoordinateFrameType CoordinateFrameType_MIN = - GLOverlay_CoordinateFrameType_CoordinateFrameType_MIN; - static const CoordinateFrameType CoordinateFrameType_MAX = - GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX; - static const int CoordinateFrameType_ARRAYSIZE = - GLOverlay_CoordinateFrameType_CoordinateFrameType_ARRAYSIZE; - static inline const ::google::protobuf::EnumDescriptor* - CoordinateFrameType_descriptor() { - return GLOverlay_CoordinateFrameType_descriptor(); - } - static inline const ::std::string& CoordinateFrameType_Name(CoordinateFrameType value) { - return GLOverlay_CoordinateFrameType_Name(value); - } - static inline bool CoordinateFrameType_Parse(const ::std::string& name, - CoordinateFrameType* value) { - return GLOverlay_CoordinateFrameType_Parse(name, value); - } - - typedef GLOverlay_Mode Mode; - static const Mode POINTS = GLOverlay_Mode_POINTS; - static const Mode LINES = GLOverlay_Mode_LINES; - static const Mode LINE_STRIP = GLOverlay_Mode_LINE_STRIP; - static const Mode LINE_LOOP = GLOverlay_Mode_LINE_LOOP; - static const Mode TRIANGLES = GLOverlay_Mode_TRIANGLES; - static const Mode TRIANGLE_STRIP = GLOverlay_Mode_TRIANGLE_STRIP; - static const Mode TRIANGLE_FAN = GLOverlay_Mode_TRIANGLE_FAN; - static const Mode QUADS = GLOverlay_Mode_QUADS; - static const Mode QUAD_STRIP = GLOverlay_Mode_QUAD_STRIP; - static const Mode POLYGON = GLOverlay_Mode_POLYGON; - static const Mode SOLID_CIRCLE = GLOverlay_Mode_SOLID_CIRCLE; - static const Mode WIRE_CIRCLE = GLOverlay_Mode_WIRE_CIRCLE; - static const Mode SOLID_CUBE = GLOverlay_Mode_SOLID_CUBE; - static const Mode WIRE_CUBE = GLOverlay_Mode_WIRE_CUBE; - static inline bool Mode_IsValid(int value) { - return GLOverlay_Mode_IsValid(value); - } - static const Mode Mode_MIN = - GLOverlay_Mode_Mode_MIN; - static const Mode Mode_MAX = - GLOverlay_Mode_Mode_MAX; - static const int Mode_ARRAYSIZE = - GLOverlay_Mode_Mode_ARRAYSIZE; - static inline const ::google::protobuf::EnumDescriptor* - Mode_descriptor() { - return GLOverlay_Mode_descriptor(); - } - static inline const ::std::string& Mode_Name(Mode value) { - return GLOverlay_Mode_Name(value); - } - static inline bool Mode_Parse(const ::std::string& name, - Mode* value) { - return GLOverlay_Mode_Parse(name, value); - } - - typedef GLOverlay_Identifier Identifier; - static const Identifier END = GLOverlay_Identifier_END; - static const Identifier VERTEX2F = GLOverlay_Identifier_VERTEX2F; - static const Identifier VERTEX3F = GLOverlay_Identifier_VERTEX3F; - static const Identifier ROTATEF = GLOverlay_Identifier_ROTATEF; - static const Identifier TRANSLATEF = GLOverlay_Identifier_TRANSLATEF; - static const Identifier SCALEF = GLOverlay_Identifier_SCALEF; - static const Identifier PUSH_MATRIX = GLOverlay_Identifier_PUSH_MATRIX; - static const Identifier POP_MATRIX = GLOverlay_Identifier_POP_MATRIX; - static const Identifier COLOR3F = GLOverlay_Identifier_COLOR3F; - static const Identifier COLOR4F = GLOverlay_Identifier_COLOR4F; - static const Identifier POINTSIZE = GLOverlay_Identifier_POINTSIZE; - static const Identifier LINEWIDTH = GLOverlay_Identifier_LINEWIDTH; - static inline bool Identifier_IsValid(int value) { - return GLOverlay_Identifier_IsValid(value); - } - static const Identifier Identifier_MIN = - GLOverlay_Identifier_Identifier_MIN; - static const Identifier Identifier_MAX = - GLOverlay_Identifier_Identifier_MAX; - static const int Identifier_ARRAYSIZE = - GLOverlay_Identifier_Identifier_ARRAYSIZE; - static inline const ::google::protobuf::EnumDescriptor* - Identifier_descriptor() { - return GLOverlay_Identifier_descriptor(); - } - static inline const ::std::string& Identifier_Name(Identifier value) { - return GLOverlay_Identifier_Name(value); - } - static inline bool Identifier_Parse(const ::std::string& name, - Identifier* value) { - return GLOverlay_Identifier_Parse(name, value); - } - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // optional string name = 2; - inline bool has_name() const; - inline void clear_name(); - static const int kNameFieldNumber = 2; - inline const ::std::string& name() const; - inline void set_name(const ::std::string& value); - inline void set_name(const char* value); - inline void set_name(const char* value, size_t size); - inline ::std::string* mutable_name(); - inline ::std::string* release_name(); - - // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; - inline bool has_coordinateframetype() const; - inline void clear_coordinateframetype(); - static const int kCoordinateFrameTypeFieldNumber = 3; - inline ::px::GLOverlay_CoordinateFrameType coordinateframetype() const; - inline void set_coordinateframetype(::px::GLOverlay_CoordinateFrameType value); - - // optional double origin_x = 4; - inline bool has_origin_x() const; - inline void clear_origin_x(); - static const int kOriginXFieldNumber = 4; - inline double origin_x() const; - inline void set_origin_x(double value); - - // optional double origin_y = 5; - inline bool has_origin_y() const; - inline void clear_origin_y(); - static const int kOriginYFieldNumber = 5; - inline double origin_y() const; - inline void set_origin_y(double value); - - // optional double origin_z = 6; - inline bool has_origin_z() const; - inline void clear_origin_z(); - static const int kOriginZFieldNumber = 6; - inline double origin_z() const; - inline void set_origin_z(double value); - - // optional bytes data = 7; - inline bool has_data() const; - inline void clear_data(); - static const int kDataFieldNumber = 7; - inline const ::std::string& data() const; - inline void set_data(const ::std::string& value); - inline void set_data(const char* value); - inline void set_data(const void* value, size_t size); - inline ::std::string* mutable_data(); - inline ::std::string* release_data(); - - // @@protoc_insertion_point(class_scope:px.GLOverlay) - private: - inline void set_has_header(); - inline void clear_has_header(); - inline void set_has_name(); - inline void clear_has_name(); - inline void set_has_coordinateframetype(); - inline void clear_has_coordinateframetype(); - inline void set_has_origin_x(); - inline void clear_has_origin_x(); - inline void set_has_origin_y(); - inline void clear_has_origin_y(); - inline void set_has_origin_z(); - inline void clear_has_origin_z(); - inline void set_has_data(); - inline void clear_has_data(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::std::string* name_; - double origin_x_; - double origin_y_; - double origin_z_; - ::std::string* data_; - int coordinateframetype_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(7 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static GLOverlay* default_instance_; -}; -// ------------------------------------------------------------------- - -class Obstacle : public ::google::protobuf::Message { - public: - Obstacle(); - virtual ~Obstacle(); - - Obstacle(const Obstacle& from); - - inline Obstacle& operator=(const Obstacle& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const Obstacle& default_instance(); - - void Swap(Obstacle* other); - - // implements Message ---------------------------------------------- - - Obstacle* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const Obstacle& from); - void MergeFrom(const Obstacle& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // optional float x = 1; - inline bool has_x() const; - inline void clear_x(); - static const int kXFieldNumber = 1; - inline float x() const; - inline void set_x(float value); - - // optional float y = 2; - inline bool has_y() const; - inline void clear_y(); - static const int kYFieldNumber = 2; - inline float y() const; - inline void set_y(float value); - - // optional float z = 3; - inline bool has_z() const; - inline void clear_z(); - static const int kZFieldNumber = 3; - inline float z() const; - inline void set_z(float value); - - // optional float length = 4; - inline bool has_length() const; - inline void clear_length(); - static const int kLengthFieldNumber = 4; - inline float length() const; - inline void set_length(float value); - - // optional float width = 5; - inline bool has_width() const; - inline void clear_width(); - static const int kWidthFieldNumber = 5; - inline float width() const; - inline void set_width(float value); - - // optional float height = 6; - inline bool has_height() const; - inline void clear_height(); - static const int kHeightFieldNumber = 6; - inline float height() const; - inline void set_height(float value); - - // @@protoc_insertion_point(class_scope:px.Obstacle) - private: - inline void set_has_x(); - inline void clear_has_x(); - inline void set_has_y(); - inline void clear_has_y(); - inline void set_has_z(); - inline void clear_has_z(); - inline void set_has_length(); - inline void clear_has_length(); - inline void set_has_width(); - inline void clear_has_width(); - inline void set_has_height(); - inline void clear_has_height(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - float x_; - float y_; - float z_; - float length_; - float width_; - float height_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(6 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static Obstacle* default_instance_; -}; -// ------------------------------------------------------------------- - -class ObstacleList : public ::google::protobuf::Message { - public: - ObstacleList(); - virtual ~ObstacleList(); - - ObstacleList(const ObstacleList& from); - - inline ObstacleList& operator=(const ObstacleList& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const ObstacleList& default_instance(); - - void Swap(ObstacleList* other); - - // implements Message ---------------------------------------------- - - ObstacleList* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const ObstacleList& from); - void MergeFrom(const ObstacleList& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // repeated .px.Obstacle obstacles = 2; - inline int obstacles_size() const; - inline void clear_obstacles(); - static const int kObstaclesFieldNumber = 2; - inline const ::px::Obstacle& obstacles(int index) const; - inline ::px::Obstacle* mutable_obstacles(int index); - inline ::px::Obstacle* add_obstacles(); - inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >& - obstacles() const; - inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >* - mutable_obstacles(); - - // @@protoc_insertion_point(class_scope:px.ObstacleList) - private: - inline void set_has_header(); - inline void clear_has_header(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::RepeatedPtrField< ::px::Obstacle > obstacles_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static ObstacleList* default_instance_; -}; -// ------------------------------------------------------------------- - -class ObstacleMap : public ::google::protobuf::Message { - public: - ObstacleMap(); - virtual ~ObstacleMap(); - - ObstacleMap(const ObstacleMap& from); - - inline ObstacleMap& operator=(const ObstacleMap& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const ObstacleMap& default_instance(); - - void Swap(ObstacleMap* other); - - // implements Message ---------------------------------------------- - - ObstacleMap* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const ObstacleMap& from); - void MergeFrom(const ObstacleMap& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // required int32 type = 2; - inline bool has_type() const; - inline void clear_type(); - static const int kTypeFieldNumber = 2; - inline ::google::protobuf::int32 type() const; - inline void set_type(::google::protobuf::int32 value); - - // optional float resolution = 3; - inline bool has_resolution() const; - inline void clear_resolution(); - static const int kResolutionFieldNumber = 3; - inline float resolution() const; - inline void set_resolution(float value); - - // optional int32 rows = 4; - inline bool has_rows() const; - inline void clear_rows(); - static const int kRowsFieldNumber = 4; - inline ::google::protobuf::int32 rows() const; - inline void set_rows(::google::protobuf::int32 value); - - // optional int32 cols = 5; - inline bool has_cols() const; - inline void clear_cols(); - static const int kColsFieldNumber = 5; - inline ::google::protobuf::int32 cols() const; - inline void set_cols(::google::protobuf::int32 value); - - // optional int32 mapR0 = 6; - inline bool has_mapr0() const; - inline void clear_mapr0(); - static const int kMapR0FieldNumber = 6; - inline ::google::protobuf::int32 mapr0() const; - inline void set_mapr0(::google::protobuf::int32 value); - - // optional int32 mapC0 = 7; - inline bool has_mapc0() const; - inline void clear_mapc0(); - static const int kMapC0FieldNumber = 7; - inline ::google::protobuf::int32 mapc0() const; - inline void set_mapc0(::google::protobuf::int32 value); - - // optional int32 arrayR0 = 8; - inline bool has_arrayr0() const; - inline void clear_arrayr0(); - static const int kArrayR0FieldNumber = 8; - inline ::google::protobuf::int32 arrayr0() const; - inline void set_arrayr0(::google::protobuf::int32 value); - - // optional int32 arrayC0 = 9; - inline bool has_arrayc0() const; - inline void clear_arrayc0(); - static const int kArrayC0FieldNumber = 9; - inline ::google::protobuf::int32 arrayc0() const; - inline void set_arrayc0(::google::protobuf::int32 value); - - // optional bytes data = 10; - inline bool has_data() const; - inline void clear_data(); - static const int kDataFieldNumber = 10; - inline const ::std::string& data() const; - inline void set_data(const ::std::string& value); - inline void set_data(const char* value); - inline void set_data(const void* value, size_t size); - inline ::std::string* mutable_data(); - inline ::std::string* release_data(); - - // @@protoc_insertion_point(class_scope:px.ObstacleMap) - private: - inline void set_has_header(); - inline void clear_has_header(); - inline void set_has_type(); - inline void clear_has_type(); - inline void set_has_resolution(); - inline void clear_has_resolution(); - inline void set_has_rows(); - inline void clear_has_rows(); - inline void set_has_cols(); - inline void clear_has_cols(); - inline void set_has_mapr0(); - inline void clear_has_mapr0(); - inline void set_has_mapc0(); - inline void clear_has_mapc0(); - inline void set_has_arrayr0(); - inline void clear_has_arrayr0(); - inline void set_has_arrayc0(); - inline void clear_has_arrayc0(); - inline void set_has_data(); - inline void clear_has_data(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::int32 type_; - float resolution_; - ::google::protobuf::int32 rows_; - ::google::protobuf::int32 cols_; - ::google::protobuf::int32 mapr0_; - ::google::protobuf::int32 mapc0_; - ::google::protobuf::int32 arrayr0_; - ::google::protobuf::int32 arrayc0_; - ::std::string* data_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(10 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static ObstacleMap* default_instance_; -}; -// ------------------------------------------------------------------- - -class Path : public ::google::protobuf::Message { - public: - Path(); - virtual ~Path(); - - Path(const Path& from); - - inline Path& operator=(const Path& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const Path& default_instance(); - - void Swap(Path* other); - - // implements Message ---------------------------------------------- - - Path* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const Path& from); - void MergeFrom(const Path& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // repeated .px.Waypoint waypoints = 2; - inline int waypoints_size() const; - inline void clear_waypoints(); - static const int kWaypointsFieldNumber = 2; - inline const ::px::Waypoint& waypoints(int index) const; - inline ::px::Waypoint* mutable_waypoints(int index); - inline ::px::Waypoint* add_waypoints(); - inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >& - waypoints() const; - inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >* - mutable_waypoints(); - - // @@protoc_insertion_point(class_scope:px.Path) - private: - inline void set_has_header(); - inline void clear_has_header(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::RepeatedPtrField< ::px::Waypoint > waypoints_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static Path* default_instance_; -}; -// ------------------------------------------------------------------- - -class PointCloudXYZI_PointXYZI : public ::google::protobuf::Message { - public: - PointCloudXYZI_PointXYZI(); - virtual ~PointCloudXYZI_PointXYZI(); - - PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from); - - inline PointCloudXYZI_PointXYZI& operator=(const PointCloudXYZI_PointXYZI& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const PointCloudXYZI_PointXYZI& default_instance(); - - void Swap(PointCloudXYZI_PointXYZI* other); - - // implements Message ---------------------------------------------- - - PointCloudXYZI_PointXYZI* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const PointCloudXYZI_PointXYZI& from); - void MergeFrom(const PointCloudXYZI_PointXYZI& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required float x = 1; - inline bool has_x() const; - inline void clear_x(); - static const int kXFieldNumber = 1; - inline float x() const; - inline void set_x(float value); - - // required float y = 2; - inline bool has_y() const; - inline void clear_y(); - static const int kYFieldNumber = 2; - inline float y() const; - inline void set_y(float value); - - // required float z = 3; - inline bool has_z() const; - inline void clear_z(); - static const int kZFieldNumber = 3; - inline float z() const; - inline void set_z(float value); - - // required float intensity = 4; - inline bool has_intensity() const; - inline void clear_intensity(); - static const int kIntensityFieldNumber = 4; - inline float intensity() const; - inline void set_intensity(float value); - - // @@protoc_insertion_point(class_scope:px.PointCloudXYZI.PointXYZI) - private: - inline void set_has_x(); - inline void clear_has_x(); - inline void set_has_y(); - inline void clear_has_y(); - inline void set_has_z(); - inline void clear_has_z(); - inline void set_has_intensity(); - inline void clear_has_intensity(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - float x_; - float y_; - float z_; - float intensity_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(4 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static PointCloudXYZI_PointXYZI* default_instance_; -}; -// ------------------------------------------------------------------- - -class PointCloudXYZI : public ::google::protobuf::Message { - public: - PointCloudXYZI(); - virtual ~PointCloudXYZI(); - - PointCloudXYZI(const PointCloudXYZI& from); - - inline PointCloudXYZI& operator=(const PointCloudXYZI& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const PointCloudXYZI& default_instance(); - - void Swap(PointCloudXYZI* other); - - // implements Message ---------------------------------------------- - - PointCloudXYZI* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const PointCloudXYZI& from); - void MergeFrom(const PointCloudXYZI& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - typedef PointCloudXYZI_PointXYZI PointXYZI; - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // repeated .px.PointCloudXYZI.PointXYZI points = 2; - inline int points_size() const; - inline void clear_points(); - static const int kPointsFieldNumber = 2; - inline const ::px::PointCloudXYZI_PointXYZI& points(int index) const; - inline ::px::PointCloudXYZI_PointXYZI* mutable_points(int index); - inline ::px::PointCloudXYZI_PointXYZI* add_points(); - inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >& - points() const; - inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >* - mutable_points(); - - // @@protoc_insertion_point(class_scope:px.PointCloudXYZI) - private: - inline void set_has_header(); - inline void clear_has_header(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI > points_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static PointCloudXYZI* default_instance_; -}; -// ------------------------------------------------------------------- - -class PointCloudXYZRGB_PointXYZRGB : public ::google::protobuf::Message { - public: - PointCloudXYZRGB_PointXYZRGB(); - virtual ~PointCloudXYZRGB_PointXYZRGB(); - - PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from); - - inline PointCloudXYZRGB_PointXYZRGB& operator=(const PointCloudXYZRGB_PointXYZRGB& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const PointCloudXYZRGB_PointXYZRGB& default_instance(); - - void Swap(PointCloudXYZRGB_PointXYZRGB* other); - - // implements Message ---------------------------------------------- - - PointCloudXYZRGB_PointXYZRGB* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from); - void MergeFrom(const PointCloudXYZRGB_PointXYZRGB& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required float x = 1; - inline bool has_x() const; - inline void clear_x(); - static const int kXFieldNumber = 1; - inline float x() const; - inline void set_x(float value); - - // required float y = 2; - inline bool has_y() const; - inline void clear_y(); - static const int kYFieldNumber = 2; - inline float y() const; - inline void set_y(float value); - - // required float z = 3; - inline bool has_z() const; - inline void clear_z(); - static const int kZFieldNumber = 3; - inline float z() const; - inline void set_z(float value); - - // required float rgb = 4; - inline bool has_rgb() const; - inline void clear_rgb(); - static const int kRgbFieldNumber = 4; - inline float rgb() const; - inline void set_rgb(float value); - - // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB.PointXYZRGB) - private: - inline void set_has_x(); - inline void clear_has_x(); - inline void set_has_y(); - inline void clear_has_y(); - inline void set_has_z(); - inline void clear_has_z(); - inline void set_has_rgb(); - inline void clear_has_rgb(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - float x_; - float y_; - float z_; - float rgb_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(4 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static PointCloudXYZRGB_PointXYZRGB* default_instance_; -}; -// ------------------------------------------------------------------- - -class PointCloudXYZRGB : public ::google::protobuf::Message { - public: - PointCloudXYZRGB(); - virtual ~PointCloudXYZRGB(); - - PointCloudXYZRGB(const PointCloudXYZRGB& from); - - inline PointCloudXYZRGB& operator=(const PointCloudXYZRGB& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const PointCloudXYZRGB& default_instance(); - - void Swap(PointCloudXYZRGB* other); - - // implements Message ---------------------------------------------- - - PointCloudXYZRGB* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const PointCloudXYZRGB& from); - void MergeFrom(const PointCloudXYZRGB& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - typedef PointCloudXYZRGB_PointXYZRGB PointXYZRGB; - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; - inline int points_size() const; - inline void clear_points(); - static const int kPointsFieldNumber = 2; - inline const ::px::PointCloudXYZRGB_PointXYZRGB& points(int index) const; - inline ::px::PointCloudXYZRGB_PointXYZRGB* mutable_points(int index); - inline ::px::PointCloudXYZRGB_PointXYZRGB* add_points(); - inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >& - points() const; - inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >* - mutable_points(); - - // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB) - private: - inline void set_has_header(); - inline void clear_has_header(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB > points_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static PointCloudXYZRGB* default_instance_; -}; -// ------------------------------------------------------------------- - -class RGBDImage : public ::google::protobuf::Message { - public: - RGBDImage(); - virtual ~RGBDImage(); - - RGBDImage(const RGBDImage& from); - - inline RGBDImage& operator=(const RGBDImage& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const RGBDImage& default_instance(); - - void Swap(RGBDImage* other); - - // implements Message ---------------------------------------------- - - RGBDImage* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const RGBDImage& from); - void MergeFrom(const RGBDImage& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required .px.HeaderInfo header = 1; - inline bool has_header() const; - inline void clear_header(); - static const int kHeaderFieldNumber = 1; - inline const ::px::HeaderInfo& header() const; - inline ::px::HeaderInfo* mutable_header(); - inline ::px::HeaderInfo* release_header(); - - // required uint32 cols = 2; - inline bool has_cols() const; - inline void clear_cols(); - static const int kColsFieldNumber = 2; - inline ::google::protobuf::uint32 cols() const; - inline void set_cols(::google::protobuf::uint32 value); - - // required uint32 rows = 3; - inline bool has_rows() const; - inline void clear_rows(); - static const int kRowsFieldNumber = 3; - inline ::google::protobuf::uint32 rows() const; - inline void set_rows(::google::protobuf::uint32 value); - - // required uint32 step1 = 4; - inline bool has_step1() const; - inline void clear_step1(); - static const int kStep1FieldNumber = 4; - inline ::google::protobuf::uint32 step1() const; - inline void set_step1(::google::protobuf::uint32 value); - - // required uint32 type1 = 5; - inline bool has_type1() const; - inline void clear_type1(); - static const int kType1FieldNumber = 5; - inline ::google::protobuf::uint32 type1() const; - inline void set_type1(::google::protobuf::uint32 value); - - // required bytes imageData1 = 6; - inline bool has_imagedata1() const; - inline void clear_imagedata1(); - static const int kImageData1FieldNumber = 6; - inline const ::std::string& imagedata1() const; - inline void set_imagedata1(const ::std::string& value); - inline void set_imagedata1(const char* value); - inline void set_imagedata1(const void* value, size_t size); - inline ::std::string* mutable_imagedata1(); - inline ::std::string* release_imagedata1(); - - // required uint32 step2 = 7; - inline bool has_step2() const; - inline void clear_step2(); - static const int kStep2FieldNumber = 7; - inline ::google::protobuf::uint32 step2() const; - inline void set_step2(::google::protobuf::uint32 value); - - // required uint32 type2 = 8; - inline bool has_type2() const; - inline void clear_type2(); - static const int kType2FieldNumber = 8; - inline ::google::protobuf::uint32 type2() const; - inline void set_type2(::google::protobuf::uint32 value); - - // required bytes imageData2 = 9; - inline bool has_imagedata2() const; - inline void clear_imagedata2(); - static const int kImageData2FieldNumber = 9; - inline const ::std::string& imagedata2() const; - inline void set_imagedata2(const ::std::string& value); - inline void set_imagedata2(const char* value); - inline void set_imagedata2(const void* value, size_t size); - inline ::std::string* mutable_imagedata2(); - inline ::std::string* release_imagedata2(); - - // optional uint32 camera_config = 10; - inline bool has_camera_config() const; - inline void clear_camera_config(); - static const int kCameraConfigFieldNumber = 10; - inline ::google::protobuf::uint32 camera_config() const; - inline void set_camera_config(::google::protobuf::uint32 value); - - // optional uint32 camera_type = 11; - inline bool has_camera_type() const; - inline void clear_camera_type(); - static const int kCameraTypeFieldNumber = 11; - inline ::google::protobuf::uint32 camera_type() const; - inline void set_camera_type(::google::protobuf::uint32 value); - - // optional float roll = 12; - inline bool has_roll() const; - inline void clear_roll(); - static const int kRollFieldNumber = 12; - inline float roll() const; - inline void set_roll(float value); - - // optional float pitch = 13; - inline bool has_pitch() const; - inline void clear_pitch(); - static const int kPitchFieldNumber = 13; - inline float pitch() const; - inline void set_pitch(float value); - - // optional float yaw = 14; - inline bool has_yaw() const; - inline void clear_yaw(); - static const int kYawFieldNumber = 14; - inline float yaw() const; - inline void set_yaw(float value); - - // optional float lon = 15; - inline bool has_lon() const; - inline void clear_lon(); - static const int kLonFieldNumber = 15; - inline float lon() const; - inline void set_lon(float value); - - // optional float lat = 16; - inline bool has_lat() const; - inline void clear_lat(); - static const int kLatFieldNumber = 16; - inline float lat() const; - inline void set_lat(float value); - - // optional float alt = 17; - inline bool has_alt() const; - inline void clear_alt(); - static const int kAltFieldNumber = 17; - inline float alt() const; - inline void set_alt(float value); - - // optional float ground_x = 18; - inline bool has_ground_x() const; - inline void clear_ground_x(); - static const int kGroundXFieldNumber = 18; - inline float ground_x() const; - inline void set_ground_x(float value); - - // optional float ground_y = 19; - inline bool has_ground_y() const; - inline void clear_ground_y(); - static const int kGroundYFieldNumber = 19; - inline float ground_y() const; - inline void set_ground_y(float value); - - // optional float ground_z = 20; - inline bool has_ground_z() const; - inline void clear_ground_z(); - static const int kGroundZFieldNumber = 20; - inline float ground_z() const; - inline void set_ground_z(float value); - - // repeated float camera_matrix = 21; - inline int camera_matrix_size() const; - inline void clear_camera_matrix(); - static const int kCameraMatrixFieldNumber = 21; - inline float camera_matrix(int index) const; - inline void set_camera_matrix(int index, float value); - inline void add_camera_matrix(float value); - inline const ::google::protobuf::RepeatedField< float >& - camera_matrix() const; - inline ::google::protobuf::RepeatedField< float >* - mutable_camera_matrix(); - - // @@protoc_insertion_point(class_scope:px.RGBDImage) - private: - inline void set_has_header(); - inline void clear_has_header(); - inline void set_has_cols(); - inline void clear_has_cols(); - inline void set_has_rows(); - inline void clear_has_rows(); - inline void set_has_step1(); - inline void clear_has_step1(); - inline void set_has_type1(); - inline void clear_has_type1(); - inline void set_has_imagedata1(); - inline void clear_has_imagedata1(); - inline void set_has_step2(); - inline void clear_has_step2(); - inline void set_has_type2(); - inline void clear_has_type2(); - inline void set_has_imagedata2(); - inline void clear_has_imagedata2(); - inline void set_has_camera_config(); - inline void clear_has_camera_config(); - inline void set_has_camera_type(); - inline void clear_has_camera_type(); - inline void set_has_roll(); - inline void clear_has_roll(); - inline void set_has_pitch(); - inline void clear_has_pitch(); - inline void set_has_yaw(); - inline void clear_has_yaw(); - inline void set_has_lon(); - inline void clear_has_lon(); - inline void set_has_lat(); - inline void clear_has_lat(); - inline void set_has_alt(); - inline void clear_has_alt(); - inline void set_has_ground_x(); - inline void clear_has_ground_x(); - inline void set_has_ground_y(); - inline void clear_has_ground_y(); - inline void set_has_ground_z(); - inline void clear_has_ground_z(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - ::px::HeaderInfo* header_; - ::google::protobuf::uint32 cols_; - ::google::protobuf::uint32 rows_; - ::google::protobuf::uint32 step1_; - ::google::protobuf::uint32 type1_; - ::std::string* imagedata1_; - ::google::protobuf::uint32 step2_; - ::google::protobuf::uint32 type2_; - ::std::string* imagedata2_; - ::google::protobuf::uint32 camera_config_; - ::google::protobuf::uint32 camera_type_; - float roll_; - float pitch_; - float yaw_; - float lon_; - float lat_; - float alt_; - float ground_x_; - float ground_y_; - ::google::protobuf::RepeatedField< float > camera_matrix_; - float ground_z_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(21 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static RGBDImage* default_instance_; -}; -// ------------------------------------------------------------------- - -class Waypoint : public ::google::protobuf::Message { - public: - Waypoint(); - virtual ~Waypoint(); - - Waypoint(const Waypoint& from); - - inline Waypoint& operator=(const Waypoint& from) { - CopyFrom(from); - return *this; - } - - inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const { - return _unknown_fields_; - } - - inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() { - return &_unknown_fields_; - } - - static const ::google::protobuf::Descriptor* descriptor(); - static const Waypoint& default_instance(); - - void Swap(Waypoint* other); - - // implements Message ---------------------------------------------- - - Waypoint* New() const; - void CopyFrom(const ::google::protobuf::Message& from); - void MergeFrom(const ::google::protobuf::Message& from); - void CopyFrom(const Waypoint& from); - void MergeFrom(const Waypoint& from); - void Clear(); - bool IsInitialized() const; - - int ByteSize() const; - bool MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input); - void SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const; - ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const; - int GetCachedSize() const { return _cached_size_; } - private: - void SharedCtor(); - void SharedDtor(); - void SetCachedSize(int size) const; - public: - - ::google::protobuf::Metadata GetMetadata() const; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // required double x = 1; - inline bool has_x() const; - inline void clear_x(); - static const int kXFieldNumber = 1; - inline double x() const; - inline void set_x(double value); - - // required double y = 2; - inline bool has_y() const; - inline void clear_y(); - static const int kYFieldNumber = 2; - inline double y() const; - inline void set_y(double value); - - // optional double z = 3; - inline bool has_z() const; - inline void clear_z(); - static const int kZFieldNumber = 3; - inline double z() const; - inline void set_z(double value); - - // optional double roll = 4; - inline bool has_roll() const; - inline void clear_roll(); - static const int kRollFieldNumber = 4; - inline double roll() const; - inline void set_roll(double value); - - // optional double pitch = 5; - inline bool has_pitch() const; - inline void clear_pitch(); - static const int kPitchFieldNumber = 5; - inline double pitch() const; - inline void set_pitch(double value); - - // optional double yaw = 6; - inline bool has_yaw() const; - inline void clear_yaw(); - static const int kYawFieldNumber = 6; - inline double yaw() const; - inline void set_yaw(double value); - - // @@protoc_insertion_point(class_scope:px.Waypoint) - private: - inline void set_has_x(); - inline void clear_has_x(); - inline void set_has_y(); - inline void clear_has_y(); - inline void set_has_z(); - inline void clear_has_z(); - inline void set_has_roll(); - inline void clear_has_roll(); - inline void set_has_pitch(); - inline void clear_has_pitch(); - inline void set_has_yaw(); - inline void clear_has_yaw(); - - ::google::protobuf::UnknownFieldSet _unknown_fields_; - - double x_; - double y_; - double z_; - double roll_; - double pitch_; - double yaw_; - - mutable int _cached_size_; - ::google::protobuf::uint32 _has_bits_[(6 + 31) / 32]; - - friend void protobuf_AddDesc_pixhawk_2eproto(); - friend void protobuf_AssignDesc_pixhawk_2eproto(); - friend void protobuf_ShutdownFile_pixhawk_2eproto(); - - void InitAsDefaultInstance(); - static Waypoint* default_instance_; -}; -// =================================================================== - - -// =================================================================== - -// HeaderInfo - -// required int32 source_sysid = 1; -inline bool HeaderInfo::has_source_sysid() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void HeaderInfo::set_has_source_sysid() { - _has_bits_[0] |= 0x00000001u; -} -inline void HeaderInfo::clear_has_source_sysid() { - _has_bits_[0] &= ~0x00000001u; -} -inline void HeaderInfo::clear_source_sysid() { - source_sysid_ = 0; - clear_has_source_sysid(); -} -inline ::google::protobuf::int32 HeaderInfo::source_sysid() const { - return source_sysid_; -} -inline void HeaderInfo::set_source_sysid(::google::protobuf::int32 value) { - set_has_source_sysid(); - source_sysid_ = value; -} - -// required int32 source_compid = 2; -inline bool HeaderInfo::has_source_compid() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void HeaderInfo::set_has_source_compid() { - _has_bits_[0] |= 0x00000002u; -} -inline void HeaderInfo::clear_has_source_compid() { - _has_bits_[0] &= ~0x00000002u; -} -inline void HeaderInfo::clear_source_compid() { - source_compid_ = 0; - clear_has_source_compid(); -} -inline ::google::protobuf::int32 HeaderInfo::source_compid() const { - return source_compid_; -} -inline void HeaderInfo::set_source_compid(::google::protobuf::int32 value) { - set_has_source_compid(); - source_compid_ = value; -} - -// required double timestamp = 3; -inline bool HeaderInfo::has_timestamp() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void HeaderInfo::set_has_timestamp() { - _has_bits_[0] |= 0x00000004u; -} -inline void HeaderInfo::clear_has_timestamp() { - _has_bits_[0] &= ~0x00000004u; -} -inline void HeaderInfo::clear_timestamp() { - timestamp_ = 0; - clear_has_timestamp(); -} -inline double HeaderInfo::timestamp() const { - return timestamp_; -} -inline void HeaderInfo::set_timestamp(double value) { - set_has_timestamp(); - timestamp_ = value; -} - -// ------------------------------------------------------------------- - -// GLOverlay - -// required .px.HeaderInfo header = 1; -inline bool GLOverlay::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void GLOverlay::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void GLOverlay::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void GLOverlay::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& GLOverlay::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* GLOverlay::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* GLOverlay::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// optional string name = 2; -inline bool GLOverlay::has_name() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void GLOverlay::set_has_name() { - _has_bits_[0] |= 0x00000002u; -} -inline void GLOverlay::clear_has_name() { - _has_bits_[0] &= ~0x00000002u; -} -inline void GLOverlay::clear_name() { - if (name_ != &::google::protobuf::internal::kEmptyString) { - name_->clear(); - } - clear_has_name(); -} -inline const ::std::string& GLOverlay::name() const { - return *name_; -} -inline void GLOverlay::set_name(const ::std::string& value) { - set_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - name_ = new ::std::string; - } - name_->assign(value); -} -inline void GLOverlay::set_name(const char* value) { - set_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - name_ = new ::std::string; - } - name_->assign(value); -} -inline void GLOverlay::set_name(const char* value, size_t size) { - set_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - name_ = new ::std::string; - } - name_->assign(reinterpret_cast(value), size); -} -inline ::std::string* GLOverlay::mutable_name() { - set_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - name_ = new ::std::string; - } - return name_; -} -inline ::std::string* GLOverlay::release_name() { - clear_has_name(); - if (name_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = name_; - name_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; -inline bool GLOverlay::has_coordinateframetype() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void GLOverlay::set_has_coordinateframetype() { - _has_bits_[0] |= 0x00000004u; -} -inline void GLOverlay::clear_has_coordinateframetype() { - _has_bits_[0] &= ~0x00000004u; -} -inline void GLOverlay::clear_coordinateframetype() { - coordinateframetype_ = 0; - clear_has_coordinateframetype(); -} -inline ::px::GLOverlay_CoordinateFrameType GLOverlay::coordinateframetype() const { - return static_cast< ::px::GLOverlay_CoordinateFrameType >(coordinateframetype_); -} -inline void GLOverlay::set_coordinateframetype(::px::GLOverlay_CoordinateFrameType value) { - GOOGLE_DCHECK(::px::GLOverlay_CoordinateFrameType_IsValid(value)); - set_has_coordinateframetype(); - coordinateframetype_ = value; -} - -// optional double origin_x = 4; -inline bool GLOverlay::has_origin_x() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void GLOverlay::set_has_origin_x() { - _has_bits_[0] |= 0x00000008u; -} -inline void GLOverlay::clear_has_origin_x() { - _has_bits_[0] &= ~0x00000008u; -} -inline void GLOverlay::clear_origin_x() { - origin_x_ = 0; - clear_has_origin_x(); -} -inline double GLOverlay::origin_x() const { - return origin_x_; -} -inline void GLOverlay::set_origin_x(double value) { - set_has_origin_x(); - origin_x_ = value; -} - -// optional double origin_y = 5; -inline bool GLOverlay::has_origin_y() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void GLOverlay::set_has_origin_y() { - _has_bits_[0] |= 0x00000010u; -} -inline void GLOverlay::clear_has_origin_y() { - _has_bits_[0] &= ~0x00000010u; -} -inline void GLOverlay::clear_origin_y() { - origin_y_ = 0; - clear_has_origin_y(); -} -inline double GLOverlay::origin_y() const { - return origin_y_; -} -inline void GLOverlay::set_origin_y(double value) { - set_has_origin_y(); - origin_y_ = value; -} - -// optional double origin_z = 6; -inline bool GLOverlay::has_origin_z() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void GLOverlay::set_has_origin_z() { - _has_bits_[0] |= 0x00000020u; -} -inline void GLOverlay::clear_has_origin_z() { - _has_bits_[0] &= ~0x00000020u; -} -inline void GLOverlay::clear_origin_z() { - origin_z_ = 0; - clear_has_origin_z(); -} -inline double GLOverlay::origin_z() const { - return origin_z_; -} -inline void GLOverlay::set_origin_z(double value) { - set_has_origin_z(); - origin_z_ = value; -} - -// optional bytes data = 7; -inline bool GLOverlay::has_data() const { - return (_has_bits_[0] & 0x00000040u) != 0; -} -inline void GLOverlay::set_has_data() { - _has_bits_[0] |= 0x00000040u; -} -inline void GLOverlay::clear_has_data() { - _has_bits_[0] &= ~0x00000040u; -} -inline void GLOverlay::clear_data() { - if (data_ != &::google::protobuf::internal::kEmptyString) { - data_->clear(); - } - clear_has_data(); -} -inline const ::std::string& GLOverlay::data() const { - return *data_; -} -inline void GLOverlay::set_data(const ::std::string& value) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(value); -} -inline void GLOverlay::set_data(const char* value) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(value); -} -inline void GLOverlay::set_data(const void* value, size_t size) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(reinterpret_cast(value), size); -} -inline ::std::string* GLOverlay::mutable_data() { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - return data_; -} -inline ::std::string* GLOverlay::release_data() { - clear_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = data_; - data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// ------------------------------------------------------------------- - -// Obstacle - -// optional float x = 1; -inline bool Obstacle::has_x() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void Obstacle::set_has_x() { - _has_bits_[0] |= 0x00000001u; -} -inline void Obstacle::clear_has_x() { - _has_bits_[0] &= ~0x00000001u; -} -inline void Obstacle::clear_x() { - x_ = 0; - clear_has_x(); -} -inline float Obstacle::x() const { - return x_; -} -inline void Obstacle::set_x(float value) { - set_has_x(); - x_ = value; -} - -// optional float y = 2; -inline bool Obstacle::has_y() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void Obstacle::set_has_y() { - _has_bits_[0] |= 0x00000002u; -} -inline void Obstacle::clear_has_y() { - _has_bits_[0] &= ~0x00000002u; -} -inline void Obstacle::clear_y() { - y_ = 0; - clear_has_y(); -} -inline float Obstacle::y() const { - return y_; -} -inline void Obstacle::set_y(float value) { - set_has_y(); - y_ = value; -} - -// optional float z = 3; -inline bool Obstacle::has_z() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void Obstacle::set_has_z() { - _has_bits_[0] |= 0x00000004u; -} -inline void Obstacle::clear_has_z() { - _has_bits_[0] &= ~0x00000004u; -} -inline void Obstacle::clear_z() { - z_ = 0; - clear_has_z(); -} -inline float Obstacle::z() const { - return z_; -} -inline void Obstacle::set_z(float value) { - set_has_z(); - z_ = value; -} - -// optional float length = 4; -inline bool Obstacle::has_length() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void Obstacle::set_has_length() { - _has_bits_[0] |= 0x00000008u; -} -inline void Obstacle::clear_has_length() { - _has_bits_[0] &= ~0x00000008u; -} -inline void Obstacle::clear_length() { - length_ = 0; - clear_has_length(); -} -inline float Obstacle::length() const { - return length_; -} -inline void Obstacle::set_length(float value) { - set_has_length(); - length_ = value; -} - -// optional float width = 5; -inline bool Obstacle::has_width() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void Obstacle::set_has_width() { - _has_bits_[0] |= 0x00000010u; -} -inline void Obstacle::clear_has_width() { - _has_bits_[0] &= ~0x00000010u; -} -inline void Obstacle::clear_width() { - width_ = 0; - clear_has_width(); -} -inline float Obstacle::width() const { - return width_; -} -inline void Obstacle::set_width(float value) { - set_has_width(); - width_ = value; -} - -// optional float height = 6; -inline bool Obstacle::has_height() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void Obstacle::set_has_height() { - _has_bits_[0] |= 0x00000020u; -} -inline void Obstacle::clear_has_height() { - _has_bits_[0] &= ~0x00000020u; -} -inline void Obstacle::clear_height() { - height_ = 0; - clear_has_height(); -} -inline float Obstacle::height() const { - return height_; -} -inline void Obstacle::set_height(float value) { - set_has_height(); - height_ = value; -} - -// ------------------------------------------------------------------- - -// ObstacleList - -// required .px.HeaderInfo header = 1; -inline bool ObstacleList::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void ObstacleList::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void ObstacleList::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void ObstacleList::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& ObstacleList::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* ObstacleList::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* ObstacleList::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// repeated .px.Obstacle obstacles = 2; -inline int ObstacleList::obstacles_size() const { - return obstacles_.size(); -} -inline void ObstacleList::clear_obstacles() { - obstacles_.Clear(); -} -inline const ::px::Obstacle& ObstacleList::obstacles(int index) const { - return obstacles_.Get(index); -} -inline ::px::Obstacle* ObstacleList::mutable_obstacles(int index) { - return obstacles_.Mutable(index); -} -inline ::px::Obstacle* ObstacleList::add_obstacles() { - return obstacles_.Add(); -} -inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >& -ObstacleList::obstacles() const { - return obstacles_; -} -inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >* -ObstacleList::mutable_obstacles() { - return &obstacles_; -} - -// ------------------------------------------------------------------- - -// ObstacleMap - -// required .px.HeaderInfo header = 1; -inline bool ObstacleMap::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void ObstacleMap::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void ObstacleMap::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void ObstacleMap::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& ObstacleMap::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* ObstacleMap::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* ObstacleMap::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// required int32 type = 2; -inline bool ObstacleMap::has_type() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void ObstacleMap::set_has_type() { - _has_bits_[0] |= 0x00000002u; -} -inline void ObstacleMap::clear_has_type() { - _has_bits_[0] &= ~0x00000002u; -} -inline void ObstacleMap::clear_type() { - type_ = 0; - clear_has_type(); -} -inline ::google::protobuf::int32 ObstacleMap::type() const { - return type_; -} -inline void ObstacleMap::set_type(::google::protobuf::int32 value) { - set_has_type(); - type_ = value; -} - -// optional float resolution = 3; -inline bool ObstacleMap::has_resolution() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void ObstacleMap::set_has_resolution() { - _has_bits_[0] |= 0x00000004u; -} -inline void ObstacleMap::clear_has_resolution() { - _has_bits_[0] &= ~0x00000004u; -} -inline void ObstacleMap::clear_resolution() { - resolution_ = 0; - clear_has_resolution(); -} -inline float ObstacleMap::resolution() const { - return resolution_; -} -inline void ObstacleMap::set_resolution(float value) { - set_has_resolution(); - resolution_ = value; -} - -// optional int32 rows = 4; -inline bool ObstacleMap::has_rows() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void ObstacleMap::set_has_rows() { - _has_bits_[0] |= 0x00000008u; -} -inline void ObstacleMap::clear_has_rows() { - _has_bits_[0] &= ~0x00000008u; -} -inline void ObstacleMap::clear_rows() { - rows_ = 0; - clear_has_rows(); -} -inline ::google::protobuf::int32 ObstacleMap::rows() const { - return rows_; -} -inline void ObstacleMap::set_rows(::google::protobuf::int32 value) { - set_has_rows(); - rows_ = value; -} - -// optional int32 cols = 5; -inline bool ObstacleMap::has_cols() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void ObstacleMap::set_has_cols() { - _has_bits_[0] |= 0x00000010u; -} -inline void ObstacleMap::clear_has_cols() { - _has_bits_[0] &= ~0x00000010u; -} -inline void ObstacleMap::clear_cols() { - cols_ = 0; - clear_has_cols(); -} -inline ::google::protobuf::int32 ObstacleMap::cols() const { - return cols_; -} -inline void ObstacleMap::set_cols(::google::protobuf::int32 value) { - set_has_cols(); - cols_ = value; -} - -// optional int32 mapR0 = 6; -inline bool ObstacleMap::has_mapr0() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void ObstacleMap::set_has_mapr0() { - _has_bits_[0] |= 0x00000020u; -} -inline void ObstacleMap::clear_has_mapr0() { - _has_bits_[0] &= ~0x00000020u; -} -inline void ObstacleMap::clear_mapr0() { - mapr0_ = 0; - clear_has_mapr0(); -} -inline ::google::protobuf::int32 ObstacleMap::mapr0() const { - return mapr0_; -} -inline void ObstacleMap::set_mapr0(::google::protobuf::int32 value) { - set_has_mapr0(); - mapr0_ = value; -} - -// optional int32 mapC0 = 7; -inline bool ObstacleMap::has_mapc0() const { - return (_has_bits_[0] & 0x00000040u) != 0; -} -inline void ObstacleMap::set_has_mapc0() { - _has_bits_[0] |= 0x00000040u; -} -inline void ObstacleMap::clear_has_mapc0() { - _has_bits_[0] &= ~0x00000040u; -} -inline void ObstacleMap::clear_mapc0() { - mapc0_ = 0; - clear_has_mapc0(); -} -inline ::google::protobuf::int32 ObstacleMap::mapc0() const { - return mapc0_; -} -inline void ObstacleMap::set_mapc0(::google::protobuf::int32 value) { - set_has_mapc0(); - mapc0_ = value; -} - -// optional int32 arrayR0 = 8; -inline bool ObstacleMap::has_arrayr0() const { - return (_has_bits_[0] & 0x00000080u) != 0; -} -inline void ObstacleMap::set_has_arrayr0() { - _has_bits_[0] |= 0x00000080u; -} -inline void ObstacleMap::clear_has_arrayr0() { - _has_bits_[0] &= ~0x00000080u; -} -inline void ObstacleMap::clear_arrayr0() { - arrayr0_ = 0; - clear_has_arrayr0(); -} -inline ::google::protobuf::int32 ObstacleMap::arrayr0() const { - return arrayr0_; -} -inline void ObstacleMap::set_arrayr0(::google::protobuf::int32 value) { - set_has_arrayr0(); - arrayr0_ = value; -} - -// optional int32 arrayC0 = 9; -inline bool ObstacleMap::has_arrayc0() const { - return (_has_bits_[0] & 0x00000100u) != 0; -} -inline void ObstacleMap::set_has_arrayc0() { - _has_bits_[0] |= 0x00000100u; -} -inline void ObstacleMap::clear_has_arrayc0() { - _has_bits_[0] &= ~0x00000100u; -} -inline void ObstacleMap::clear_arrayc0() { - arrayc0_ = 0; - clear_has_arrayc0(); -} -inline ::google::protobuf::int32 ObstacleMap::arrayc0() const { - return arrayc0_; -} -inline void ObstacleMap::set_arrayc0(::google::protobuf::int32 value) { - set_has_arrayc0(); - arrayc0_ = value; -} - -// optional bytes data = 10; -inline bool ObstacleMap::has_data() const { - return (_has_bits_[0] & 0x00000200u) != 0; -} -inline void ObstacleMap::set_has_data() { - _has_bits_[0] |= 0x00000200u; -} -inline void ObstacleMap::clear_has_data() { - _has_bits_[0] &= ~0x00000200u; -} -inline void ObstacleMap::clear_data() { - if (data_ != &::google::protobuf::internal::kEmptyString) { - data_->clear(); - } - clear_has_data(); -} -inline const ::std::string& ObstacleMap::data() const { - return *data_; -} -inline void ObstacleMap::set_data(const ::std::string& value) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(value); -} -inline void ObstacleMap::set_data(const char* value) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(value); -} -inline void ObstacleMap::set_data(const void* value, size_t size) { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - data_->assign(reinterpret_cast(value), size); -} -inline ::std::string* ObstacleMap::mutable_data() { - set_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - data_ = new ::std::string; - } - return data_; -} -inline ::std::string* ObstacleMap::release_data() { - clear_has_data(); - if (data_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = data_; - data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// ------------------------------------------------------------------- - -// Path - -// required .px.HeaderInfo header = 1; -inline bool Path::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void Path::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void Path::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void Path::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& Path::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* Path::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* Path::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// repeated .px.Waypoint waypoints = 2; -inline int Path::waypoints_size() const { - return waypoints_.size(); -} -inline void Path::clear_waypoints() { - waypoints_.Clear(); -} -inline const ::px::Waypoint& Path::waypoints(int index) const { - return waypoints_.Get(index); -} -inline ::px::Waypoint* Path::mutable_waypoints(int index) { - return waypoints_.Mutable(index); -} -inline ::px::Waypoint* Path::add_waypoints() { - return waypoints_.Add(); -} -inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >& -Path::waypoints() const { - return waypoints_; -} -inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >* -Path::mutable_waypoints() { - return &waypoints_; -} - -// ------------------------------------------------------------------- - -// PointCloudXYZI_PointXYZI - -// required float x = 1; -inline bool PointCloudXYZI_PointXYZI::has_x() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void PointCloudXYZI_PointXYZI::set_has_x() { - _has_bits_[0] |= 0x00000001u; -} -inline void PointCloudXYZI_PointXYZI::clear_has_x() { - _has_bits_[0] &= ~0x00000001u; -} -inline void PointCloudXYZI_PointXYZI::clear_x() { - x_ = 0; - clear_has_x(); -} -inline float PointCloudXYZI_PointXYZI::x() const { - return x_; -} -inline void PointCloudXYZI_PointXYZI::set_x(float value) { - set_has_x(); - x_ = value; -} - -// required float y = 2; -inline bool PointCloudXYZI_PointXYZI::has_y() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void PointCloudXYZI_PointXYZI::set_has_y() { - _has_bits_[0] |= 0x00000002u; -} -inline void PointCloudXYZI_PointXYZI::clear_has_y() { - _has_bits_[0] &= ~0x00000002u; -} -inline void PointCloudXYZI_PointXYZI::clear_y() { - y_ = 0; - clear_has_y(); -} -inline float PointCloudXYZI_PointXYZI::y() const { - return y_; -} -inline void PointCloudXYZI_PointXYZI::set_y(float value) { - set_has_y(); - y_ = value; -} - -// required float z = 3; -inline bool PointCloudXYZI_PointXYZI::has_z() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void PointCloudXYZI_PointXYZI::set_has_z() { - _has_bits_[0] |= 0x00000004u; -} -inline void PointCloudXYZI_PointXYZI::clear_has_z() { - _has_bits_[0] &= ~0x00000004u; -} -inline void PointCloudXYZI_PointXYZI::clear_z() { - z_ = 0; - clear_has_z(); -} -inline float PointCloudXYZI_PointXYZI::z() const { - return z_; -} -inline void PointCloudXYZI_PointXYZI::set_z(float value) { - set_has_z(); - z_ = value; -} - -// required float intensity = 4; -inline bool PointCloudXYZI_PointXYZI::has_intensity() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void PointCloudXYZI_PointXYZI::set_has_intensity() { - _has_bits_[0] |= 0x00000008u; -} -inline void PointCloudXYZI_PointXYZI::clear_has_intensity() { - _has_bits_[0] &= ~0x00000008u; -} -inline void PointCloudXYZI_PointXYZI::clear_intensity() { - intensity_ = 0; - clear_has_intensity(); -} -inline float PointCloudXYZI_PointXYZI::intensity() const { - return intensity_; -} -inline void PointCloudXYZI_PointXYZI::set_intensity(float value) { - set_has_intensity(); - intensity_ = value; -} - -// ------------------------------------------------------------------- - -// PointCloudXYZI - -// required .px.HeaderInfo header = 1; -inline bool PointCloudXYZI::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void PointCloudXYZI::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void PointCloudXYZI::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void PointCloudXYZI::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& PointCloudXYZI::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* PointCloudXYZI::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* PointCloudXYZI::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// repeated .px.PointCloudXYZI.PointXYZI points = 2; -inline int PointCloudXYZI::points_size() const { - return points_.size(); -} -inline void PointCloudXYZI::clear_points() { - points_.Clear(); -} -inline const ::px::PointCloudXYZI_PointXYZI& PointCloudXYZI::points(int index) const { - return points_.Get(index); -} -inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::mutable_points(int index) { - return points_.Mutable(index); -} -inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::add_points() { - return points_.Add(); -} -inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >& -PointCloudXYZI::points() const { - return points_; -} -inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >* -PointCloudXYZI::mutable_points() { - return &points_; -} - -// ------------------------------------------------------------------- - -// PointCloudXYZRGB_PointXYZRGB - -// required float x = 1; -inline bool PointCloudXYZRGB_PointXYZRGB::has_x() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_has_x() { - _has_bits_[0] |= 0x00000001u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_has_x() { - _has_bits_[0] &= ~0x00000001u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_x() { - x_ = 0; - clear_has_x(); -} -inline float PointCloudXYZRGB_PointXYZRGB::x() const { - return x_; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_x(float value) { - set_has_x(); - x_ = value; -} - -// required float y = 2; -inline bool PointCloudXYZRGB_PointXYZRGB::has_y() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_has_y() { - _has_bits_[0] |= 0x00000002u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_has_y() { - _has_bits_[0] &= ~0x00000002u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_y() { - y_ = 0; - clear_has_y(); -} -inline float PointCloudXYZRGB_PointXYZRGB::y() const { - return y_; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_y(float value) { - set_has_y(); - y_ = value; -} - -// required float z = 3; -inline bool PointCloudXYZRGB_PointXYZRGB::has_z() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_has_z() { - _has_bits_[0] |= 0x00000004u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_has_z() { - _has_bits_[0] &= ~0x00000004u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_z() { - z_ = 0; - clear_has_z(); -} -inline float PointCloudXYZRGB_PointXYZRGB::z() const { - return z_; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_z(float value) { - set_has_z(); - z_ = value; -} - -// required float rgb = 4; -inline bool PointCloudXYZRGB_PointXYZRGB::has_rgb() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_has_rgb() { - _has_bits_[0] |= 0x00000008u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_has_rgb() { - _has_bits_[0] &= ~0x00000008u; -} -inline void PointCloudXYZRGB_PointXYZRGB::clear_rgb() { - rgb_ = 0; - clear_has_rgb(); -} -inline float PointCloudXYZRGB_PointXYZRGB::rgb() const { - return rgb_; -} -inline void PointCloudXYZRGB_PointXYZRGB::set_rgb(float value) { - set_has_rgb(); - rgb_ = value; -} - -// ------------------------------------------------------------------- - -// PointCloudXYZRGB - -// required .px.HeaderInfo header = 1; -inline bool PointCloudXYZRGB::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void PointCloudXYZRGB::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void PointCloudXYZRGB::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void PointCloudXYZRGB::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& PointCloudXYZRGB::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* PointCloudXYZRGB::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* PointCloudXYZRGB::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; -inline int PointCloudXYZRGB::points_size() const { - return points_.size(); -} -inline void PointCloudXYZRGB::clear_points() { - points_.Clear(); -} -inline const ::px::PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB::points(int index) const { - return points_.Get(index); -} -inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::mutable_points(int index) { - return points_.Mutable(index); -} -inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::add_points() { - return points_.Add(); -} -inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >& -PointCloudXYZRGB::points() const { - return points_; -} -inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >* -PointCloudXYZRGB::mutable_points() { - return &points_; -} - -// ------------------------------------------------------------------- - -// RGBDImage - -// required .px.HeaderInfo header = 1; -inline bool RGBDImage::has_header() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void RGBDImage::set_has_header() { - _has_bits_[0] |= 0x00000001u; -} -inline void RGBDImage::clear_has_header() { - _has_bits_[0] &= ~0x00000001u; -} -inline void RGBDImage::clear_header() { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - clear_has_header(); -} -inline const ::px::HeaderInfo& RGBDImage::header() const { - return header_ != NULL ? *header_ : *default_instance_->header_; -} -inline ::px::HeaderInfo* RGBDImage::mutable_header() { - set_has_header(); - if (header_ == NULL) header_ = new ::px::HeaderInfo; - return header_; -} -inline ::px::HeaderInfo* RGBDImage::release_header() { - clear_has_header(); - ::px::HeaderInfo* temp = header_; - header_ = NULL; - return temp; -} - -// required uint32 cols = 2; -inline bool RGBDImage::has_cols() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void RGBDImage::set_has_cols() { - _has_bits_[0] |= 0x00000002u; -} -inline void RGBDImage::clear_has_cols() { - _has_bits_[0] &= ~0x00000002u; -} -inline void RGBDImage::clear_cols() { - cols_ = 0u; - clear_has_cols(); -} -inline ::google::protobuf::uint32 RGBDImage::cols() const { - return cols_; -} -inline void RGBDImage::set_cols(::google::protobuf::uint32 value) { - set_has_cols(); - cols_ = value; -} - -// required uint32 rows = 3; -inline bool RGBDImage::has_rows() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void RGBDImage::set_has_rows() { - _has_bits_[0] |= 0x00000004u; -} -inline void RGBDImage::clear_has_rows() { - _has_bits_[0] &= ~0x00000004u; -} -inline void RGBDImage::clear_rows() { - rows_ = 0u; - clear_has_rows(); -} -inline ::google::protobuf::uint32 RGBDImage::rows() const { - return rows_; -} -inline void RGBDImage::set_rows(::google::protobuf::uint32 value) { - set_has_rows(); - rows_ = value; -} - -// required uint32 step1 = 4; -inline bool RGBDImage::has_step1() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void RGBDImage::set_has_step1() { - _has_bits_[0] |= 0x00000008u; -} -inline void RGBDImage::clear_has_step1() { - _has_bits_[0] &= ~0x00000008u; -} -inline void RGBDImage::clear_step1() { - step1_ = 0u; - clear_has_step1(); -} -inline ::google::protobuf::uint32 RGBDImage::step1() const { - return step1_; -} -inline void RGBDImage::set_step1(::google::protobuf::uint32 value) { - set_has_step1(); - step1_ = value; -} - -// required uint32 type1 = 5; -inline bool RGBDImage::has_type1() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void RGBDImage::set_has_type1() { - _has_bits_[0] |= 0x00000010u; -} -inline void RGBDImage::clear_has_type1() { - _has_bits_[0] &= ~0x00000010u; -} -inline void RGBDImage::clear_type1() { - type1_ = 0u; - clear_has_type1(); -} -inline ::google::protobuf::uint32 RGBDImage::type1() const { - return type1_; -} -inline void RGBDImage::set_type1(::google::protobuf::uint32 value) { - set_has_type1(); - type1_ = value; -} - -// required bytes imageData1 = 6; -inline bool RGBDImage::has_imagedata1() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void RGBDImage::set_has_imagedata1() { - _has_bits_[0] |= 0x00000020u; -} -inline void RGBDImage::clear_has_imagedata1() { - _has_bits_[0] &= ~0x00000020u; -} -inline void RGBDImage::clear_imagedata1() { - if (imagedata1_ != &::google::protobuf::internal::kEmptyString) { - imagedata1_->clear(); - } - clear_has_imagedata1(); -} -inline const ::std::string& RGBDImage::imagedata1() const { - return *imagedata1_; -} -inline void RGBDImage::set_imagedata1(const ::std::string& value) { - set_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - imagedata1_ = new ::std::string; - } - imagedata1_->assign(value); -} -inline void RGBDImage::set_imagedata1(const char* value) { - set_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - imagedata1_ = new ::std::string; - } - imagedata1_->assign(value); -} -inline void RGBDImage::set_imagedata1(const void* value, size_t size) { - set_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - imagedata1_ = new ::std::string; - } - imagedata1_->assign(reinterpret_cast(value), size); -} -inline ::std::string* RGBDImage::mutable_imagedata1() { - set_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - imagedata1_ = new ::std::string; - } - return imagedata1_; -} -inline ::std::string* RGBDImage::release_imagedata1() { - clear_has_imagedata1(); - if (imagedata1_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = imagedata1_; - imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// required uint32 step2 = 7; -inline bool RGBDImage::has_step2() const { - return (_has_bits_[0] & 0x00000040u) != 0; -} -inline void RGBDImage::set_has_step2() { - _has_bits_[0] |= 0x00000040u; -} -inline void RGBDImage::clear_has_step2() { - _has_bits_[0] &= ~0x00000040u; -} -inline void RGBDImage::clear_step2() { - step2_ = 0u; - clear_has_step2(); -} -inline ::google::protobuf::uint32 RGBDImage::step2() const { - return step2_; -} -inline void RGBDImage::set_step2(::google::protobuf::uint32 value) { - set_has_step2(); - step2_ = value; -} - -// required uint32 type2 = 8; -inline bool RGBDImage::has_type2() const { - return (_has_bits_[0] & 0x00000080u) != 0; -} -inline void RGBDImage::set_has_type2() { - _has_bits_[0] |= 0x00000080u; -} -inline void RGBDImage::clear_has_type2() { - _has_bits_[0] &= ~0x00000080u; -} -inline void RGBDImage::clear_type2() { - type2_ = 0u; - clear_has_type2(); -} -inline ::google::protobuf::uint32 RGBDImage::type2() const { - return type2_; -} -inline void RGBDImage::set_type2(::google::protobuf::uint32 value) { - set_has_type2(); - type2_ = value; -} - -// required bytes imageData2 = 9; -inline bool RGBDImage::has_imagedata2() const { - return (_has_bits_[0] & 0x00000100u) != 0; -} -inline void RGBDImage::set_has_imagedata2() { - _has_bits_[0] |= 0x00000100u; -} -inline void RGBDImage::clear_has_imagedata2() { - _has_bits_[0] &= ~0x00000100u; -} -inline void RGBDImage::clear_imagedata2() { - if (imagedata2_ != &::google::protobuf::internal::kEmptyString) { - imagedata2_->clear(); - } - clear_has_imagedata2(); -} -inline const ::std::string& RGBDImage::imagedata2() const { - return *imagedata2_; -} -inline void RGBDImage::set_imagedata2(const ::std::string& value) { - set_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - imagedata2_ = new ::std::string; - } - imagedata2_->assign(value); -} -inline void RGBDImage::set_imagedata2(const char* value) { - set_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - imagedata2_ = new ::std::string; - } - imagedata2_->assign(value); -} -inline void RGBDImage::set_imagedata2(const void* value, size_t size) { - set_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - imagedata2_ = new ::std::string; - } - imagedata2_->assign(reinterpret_cast(value), size); -} -inline ::std::string* RGBDImage::mutable_imagedata2() { - set_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - imagedata2_ = new ::std::string; - } - return imagedata2_; -} -inline ::std::string* RGBDImage::release_imagedata2() { - clear_has_imagedata2(); - if (imagedata2_ == &::google::protobuf::internal::kEmptyString) { - return NULL; - } else { - ::std::string* temp = imagedata2_; - imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - return temp; - } -} - -// optional uint32 camera_config = 10; -inline bool RGBDImage::has_camera_config() const { - return (_has_bits_[0] & 0x00000200u) != 0; -} -inline void RGBDImage::set_has_camera_config() { - _has_bits_[0] |= 0x00000200u; -} -inline void RGBDImage::clear_has_camera_config() { - _has_bits_[0] &= ~0x00000200u; -} -inline void RGBDImage::clear_camera_config() { - camera_config_ = 0u; - clear_has_camera_config(); -} -inline ::google::protobuf::uint32 RGBDImage::camera_config() const { - return camera_config_; -} -inline void RGBDImage::set_camera_config(::google::protobuf::uint32 value) { - set_has_camera_config(); - camera_config_ = value; -} - -// optional uint32 camera_type = 11; -inline bool RGBDImage::has_camera_type() const { - return (_has_bits_[0] & 0x00000400u) != 0; -} -inline void RGBDImage::set_has_camera_type() { - _has_bits_[0] |= 0x00000400u; -} -inline void RGBDImage::clear_has_camera_type() { - _has_bits_[0] &= ~0x00000400u; -} -inline void RGBDImage::clear_camera_type() { - camera_type_ = 0u; - clear_has_camera_type(); -} -inline ::google::protobuf::uint32 RGBDImage::camera_type() const { - return camera_type_; -} -inline void RGBDImage::set_camera_type(::google::protobuf::uint32 value) { - set_has_camera_type(); - camera_type_ = value; -} - -// optional float roll = 12; -inline bool RGBDImage::has_roll() const { - return (_has_bits_[0] & 0x00000800u) != 0; -} -inline void RGBDImage::set_has_roll() { - _has_bits_[0] |= 0x00000800u; -} -inline void RGBDImage::clear_has_roll() { - _has_bits_[0] &= ~0x00000800u; -} -inline void RGBDImage::clear_roll() { - roll_ = 0; - clear_has_roll(); -} -inline float RGBDImage::roll() const { - return roll_; -} -inline void RGBDImage::set_roll(float value) { - set_has_roll(); - roll_ = value; -} - -// optional float pitch = 13; -inline bool RGBDImage::has_pitch() const { - return (_has_bits_[0] & 0x00001000u) != 0; -} -inline void RGBDImage::set_has_pitch() { - _has_bits_[0] |= 0x00001000u; -} -inline void RGBDImage::clear_has_pitch() { - _has_bits_[0] &= ~0x00001000u; -} -inline void RGBDImage::clear_pitch() { - pitch_ = 0; - clear_has_pitch(); -} -inline float RGBDImage::pitch() const { - return pitch_; -} -inline void RGBDImage::set_pitch(float value) { - set_has_pitch(); - pitch_ = value; -} - -// optional float yaw = 14; -inline bool RGBDImage::has_yaw() const { - return (_has_bits_[0] & 0x00002000u) != 0; -} -inline void RGBDImage::set_has_yaw() { - _has_bits_[0] |= 0x00002000u; -} -inline void RGBDImage::clear_has_yaw() { - _has_bits_[0] &= ~0x00002000u; -} -inline void RGBDImage::clear_yaw() { - yaw_ = 0; - clear_has_yaw(); -} -inline float RGBDImage::yaw() const { - return yaw_; -} -inline void RGBDImage::set_yaw(float value) { - set_has_yaw(); - yaw_ = value; -} - -// optional float lon = 15; -inline bool RGBDImage::has_lon() const { - return (_has_bits_[0] & 0x00004000u) != 0; -} -inline void RGBDImage::set_has_lon() { - _has_bits_[0] |= 0x00004000u; -} -inline void RGBDImage::clear_has_lon() { - _has_bits_[0] &= ~0x00004000u; -} -inline void RGBDImage::clear_lon() { - lon_ = 0; - clear_has_lon(); -} -inline float RGBDImage::lon() const { - return lon_; -} -inline void RGBDImage::set_lon(float value) { - set_has_lon(); - lon_ = value; -} - -// optional float lat = 16; -inline bool RGBDImage::has_lat() const { - return (_has_bits_[0] & 0x00008000u) != 0; -} -inline void RGBDImage::set_has_lat() { - _has_bits_[0] |= 0x00008000u; -} -inline void RGBDImage::clear_has_lat() { - _has_bits_[0] &= ~0x00008000u; -} -inline void RGBDImage::clear_lat() { - lat_ = 0; - clear_has_lat(); -} -inline float RGBDImage::lat() const { - return lat_; -} -inline void RGBDImage::set_lat(float value) { - set_has_lat(); - lat_ = value; -} - -// optional float alt = 17; -inline bool RGBDImage::has_alt() const { - return (_has_bits_[0] & 0x00010000u) != 0; -} -inline void RGBDImage::set_has_alt() { - _has_bits_[0] |= 0x00010000u; -} -inline void RGBDImage::clear_has_alt() { - _has_bits_[0] &= ~0x00010000u; -} -inline void RGBDImage::clear_alt() { - alt_ = 0; - clear_has_alt(); -} -inline float RGBDImage::alt() const { - return alt_; -} -inline void RGBDImage::set_alt(float value) { - set_has_alt(); - alt_ = value; -} - -// optional float ground_x = 18; -inline bool RGBDImage::has_ground_x() const { - return (_has_bits_[0] & 0x00020000u) != 0; -} -inline void RGBDImage::set_has_ground_x() { - _has_bits_[0] |= 0x00020000u; -} -inline void RGBDImage::clear_has_ground_x() { - _has_bits_[0] &= ~0x00020000u; -} -inline void RGBDImage::clear_ground_x() { - ground_x_ = 0; - clear_has_ground_x(); -} -inline float RGBDImage::ground_x() const { - return ground_x_; -} -inline void RGBDImage::set_ground_x(float value) { - set_has_ground_x(); - ground_x_ = value; -} - -// optional float ground_y = 19; -inline bool RGBDImage::has_ground_y() const { - return (_has_bits_[0] & 0x00040000u) != 0; -} -inline void RGBDImage::set_has_ground_y() { - _has_bits_[0] |= 0x00040000u; -} -inline void RGBDImage::clear_has_ground_y() { - _has_bits_[0] &= ~0x00040000u; -} -inline void RGBDImage::clear_ground_y() { - ground_y_ = 0; - clear_has_ground_y(); -} -inline float RGBDImage::ground_y() const { - return ground_y_; -} -inline void RGBDImage::set_ground_y(float value) { - set_has_ground_y(); - ground_y_ = value; -} - -// optional float ground_z = 20; -inline bool RGBDImage::has_ground_z() const { - return (_has_bits_[0] & 0x00080000u) != 0; -} -inline void RGBDImage::set_has_ground_z() { - _has_bits_[0] |= 0x00080000u; -} -inline void RGBDImage::clear_has_ground_z() { - _has_bits_[0] &= ~0x00080000u; -} -inline void RGBDImage::clear_ground_z() { - ground_z_ = 0; - clear_has_ground_z(); -} -inline float RGBDImage::ground_z() const { - return ground_z_; -} -inline void RGBDImage::set_ground_z(float value) { - set_has_ground_z(); - ground_z_ = value; -} - -// repeated float camera_matrix = 21; -inline int RGBDImage::camera_matrix_size() const { - return camera_matrix_.size(); -} -inline void RGBDImage::clear_camera_matrix() { - camera_matrix_.Clear(); -} -inline float RGBDImage::camera_matrix(int index) const { - return camera_matrix_.Get(index); -} -inline void RGBDImage::set_camera_matrix(int index, float value) { - camera_matrix_.Set(index, value); -} -inline void RGBDImage::add_camera_matrix(float value) { - camera_matrix_.Add(value); -} -inline const ::google::protobuf::RepeatedField< float >& -RGBDImage::camera_matrix() const { - return camera_matrix_; -} -inline ::google::protobuf::RepeatedField< float >* -RGBDImage::mutable_camera_matrix() { - return &camera_matrix_; -} - -// ------------------------------------------------------------------- - -// Waypoint - -// required double x = 1; -inline bool Waypoint::has_x() const { - return (_has_bits_[0] & 0x00000001u) != 0; -} -inline void Waypoint::set_has_x() { - _has_bits_[0] |= 0x00000001u; -} -inline void Waypoint::clear_has_x() { - _has_bits_[0] &= ~0x00000001u; -} -inline void Waypoint::clear_x() { - x_ = 0; - clear_has_x(); -} -inline double Waypoint::x() const { - return x_; -} -inline void Waypoint::set_x(double value) { - set_has_x(); - x_ = value; -} - -// required double y = 2; -inline bool Waypoint::has_y() const { - return (_has_bits_[0] & 0x00000002u) != 0; -} -inline void Waypoint::set_has_y() { - _has_bits_[0] |= 0x00000002u; -} -inline void Waypoint::clear_has_y() { - _has_bits_[0] &= ~0x00000002u; -} -inline void Waypoint::clear_y() { - y_ = 0; - clear_has_y(); -} -inline double Waypoint::y() const { - return y_; -} -inline void Waypoint::set_y(double value) { - set_has_y(); - y_ = value; -} - -// optional double z = 3; -inline bool Waypoint::has_z() const { - return (_has_bits_[0] & 0x00000004u) != 0; -} -inline void Waypoint::set_has_z() { - _has_bits_[0] |= 0x00000004u; -} -inline void Waypoint::clear_has_z() { - _has_bits_[0] &= ~0x00000004u; -} -inline void Waypoint::clear_z() { - z_ = 0; - clear_has_z(); -} -inline double Waypoint::z() const { - return z_; -} -inline void Waypoint::set_z(double value) { - set_has_z(); - z_ = value; -} - -// optional double roll = 4; -inline bool Waypoint::has_roll() const { - return (_has_bits_[0] & 0x00000008u) != 0; -} -inline void Waypoint::set_has_roll() { - _has_bits_[0] |= 0x00000008u; -} -inline void Waypoint::clear_has_roll() { - _has_bits_[0] &= ~0x00000008u; -} -inline void Waypoint::clear_roll() { - roll_ = 0; - clear_has_roll(); -} -inline double Waypoint::roll() const { - return roll_; -} -inline void Waypoint::set_roll(double value) { - set_has_roll(); - roll_ = value; -} - -// optional double pitch = 5; -inline bool Waypoint::has_pitch() const { - return (_has_bits_[0] & 0x00000010u) != 0; -} -inline void Waypoint::set_has_pitch() { - _has_bits_[0] |= 0x00000010u; -} -inline void Waypoint::clear_has_pitch() { - _has_bits_[0] &= ~0x00000010u; -} -inline void Waypoint::clear_pitch() { - pitch_ = 0; - clear_has_pitch(); -} -inline double Waypoint::pitch() const { - return pitch_; -} -inline void Waypoint::set_pitch(double value) { - set_has_pitch(); - pitch_ = value; -} - -// optional double yaw = 6; -inline bool Waypoint::has_yaw() const { - return (_has_bits_[0] & 0x00000020u) != 0; -} -inline void Waypoint::set_has_yaw() { - _has_bits_[0] |= 0x00000020u; -} -inline void Waypoint::clear_has_yaw() { - _has_bits_[0] &= ~0x00000020u; -} -inline void Waypoint::clear_yaw() { - yaw_ = 0; - clear_has_yaw(); -} -inline double Waypoint::yaw() const { - return yaw_; -} -inline void Waypoint::set_yaw(double value) { - set_has_yaw(); - yaw_ = value; -} - - -// @@protoc_insertion_point(namespace_scope) - -} // namespace px - -#ifndef SWIG -namespace google { -namespace protobuf { - -template <> -inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_CoordinateFrameType>() { - return ::px::GLOverlay_CoordinateFrameType_descriptor(); -} -template <> -inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_Mode>() { - return ::px::GLOverlay_Mode_descriptor(); -} -template <> -inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_Identifier>() { - return ::px::GLOverlay_Identifier_descriptor(); -} - -} // namespace google -} // namespace protobuf -#endif // SWIG - -// @@protoc_insertion_point(global_scope) - -#endif // PROTOBUF_pixhawk_2eproto__INCLUDED diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/protocol.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/protocol.h deleted file mode 100644 index 7b3e3c0bd..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/protocol.h +++ /dev/null @@ -1,322 +0,0 @@ -#ifndef _MAVLINK_PROTOCOL_H_ -#define _MAVLINK_PROTOCOL_H_ - -#include "string.h" -#include "mavlink_types.h" - -/* - If you want MAVLink on a system that is native big-endian, - you need to define NATIVE_BIG_ENDIAN -*/ -#ifdef NATIVE_BIG_ENDIAN -# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN) -#else -# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN) -#endif - -#ifndef MAVLINK_STACK_BUFFER -#define MAVLINK_STACK_BUFFER 0 -#endif - -#ifndef MAVLINK_AVOID_GCC_STACK_BUG -# define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__) -#endif - -#ifndef MAVLINK_ASSERT -#define MAVLINK_ASSERT(x) -#endif - -#ifndef MAVLINK_START_UART_SEND -#define MAVLINK_START_UART_SEND(chan, length) -#endif - -#ifndef MAVLINK_END_UART_SEND -#define MAVLINK_END_UART_SEND(chan, length) -#endif - -#ifdef MAVLINK_SEPARATE_HELPERS -#define MAVLINK_HELPER -#else -#define MAVLINK_HELPER static inline -#include "mavlink_helpers.h" -#endif // MAVLINK_SEPARATE_HELPERS - -/* always include the prototypes to ensure we don't get out of sync */ -MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan); -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length, uint8_t crc_extra); -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length, uint8_t crc_extra); -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, - uint8_t length, uint8_t crc_extra); -#endif -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length); -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length); -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length); -#endif // MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg); -MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg); -MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c); -MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status); -MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, - uint8_t* r_bit_index, uint8_t* buffer); -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); -#endif - -/** - * @brief Get the required buffer size for this message - */ -static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) -{ - return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; -} - -#if MAVLINK_NEED_BYTE_SWAP -static inline void byte_swap_2(char *dst, const char *src) -{ - dst[0] = src[1]; - dst[1] = src[0]; -} -static inline void byte_swap_4(char *dst, const char *src) -{ - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; -} -static inline void byte_swap_8(char *dst, const char *src) -{ - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; -} -#elif !MAVLINK_ALIGNED_FIELDS -static inline void byte_copy_2(char *dst, const char *src) -{ - dst[0] = src[0]; - dst[1] = src[1]; -} -static inline void byte_copy_4(char *dst, const char *src) -{ - dst[0] = src[0]; - dst[1] = src[1]; - dst[2] = src[2]; - dst[3] = src[3]; -} -static inline void byte_copy_8(char *dst, const char *src) -{ - memcpy(dst, src, 8); -} -#endif - -#define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b -#define _mav_put_int8_t(buf, wire_offset, b) buf[wire_offset] = (int8_t)b -#define _mav_put_char(buf, wire_offset, b) buf[wire_offset] = b - -#if MAVLINK_NEED_BYTE_SWAP -#define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b) -#define _mav_put_int16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b) -#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_int32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) -#define _mav_put_int64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) -#define _mav_put_float(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_double(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) -#elif !MAVLINK_ALIGNED_FIELDS -#define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b) -#define _mav_put_int16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b) -#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_int32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) -#define _mav_put_int64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) -#define _mav_put_float(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) -#define _mav_put_double(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) -#else -#define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b -#define _mav_put_int16_t(buf, wire_offset, b) *(int16_t *)&buf[wire_offset] = b -#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b -#define _mav_put_int32_t(buf, wire_offset, b) *(int32_t *)&buf[wire_offset] = b -#define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b -#define _mav_put_int64_t(buf, wire_offset, b) *(int64_t *)&buf[wire_offset] = b -#define _mav_put_float(buf, wire_offset, b) *(float *)&buf[wire_offset] = b -#define _mav_put_double(buf, wire_offset, b) *(double *)&buf[wire_offset] = b -#endif - -/* - like memcpy(), but if src is NULL, do a memset to zero -*/ -static void mav_array_memcpy(void *dest, const void *src, size_t n) -{ - if (src == NULL) { - memset(dest, 0, n); - } else { - memcpy(dest, src, n); - } -} - -/* - * Place a char array into a buffer - */ -static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length) -{ - mav_array_memcpy(&buf[wire_offset], b, array_length); - -} - -/* - * Place a uint8_t array into a buffer - */ -static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length) -{ - mav_array_memcpy(&buf[wire_offset], b, array_length); - -} - -/* - * Place a int8_t array into a buffer - */ -static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length) -{ - mav_array_memcpy(&buf[wire_offset], b, array_length); - -} - -#if MAVLINK_NEED_BYTE_SWAP -#define _MAV_PUT_ARRAY(TYPE, V) \ -static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ -{ \ - if (b == NULL) { \ - memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \ - } else { \ - uint16_t i; \ - for (i=0; imsgid = MAVLINK_MSG_ID_TEST_TYPES; - return mavlink_finalize_message(msg, system_id, component_id, 179, 103); -} - -/** - * @brief Pack a test_types message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param c char - * @param s string - * @param u8 uint8_t - * @param u16 uint16_t - * @param u32 uint32_t - * @param u64 uint64_t - * @param s8 int8_t - * @param s16 int16_t - * @param s32 int32_t - * @param s64 int64_t - * @param f float - * @param d double - * @param u8_array uint8_t_array - * @param u16_array uint16_t_array - * @param u32_array uint32_t_array - * @param u64_array uint64_t_array - * @param s8_array int8_t_array - * @param s16_array int16_t_array - * @param s32_array int32_t_array - * @param s64_array int64_t_array - * @param f_array float_array - * @param d_array double_array - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_test_types_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - char c,const char *s,uint8_t u8,uint16_t u16,uint32_t u32,uint64_t u64,int8_t s8,int16_t s16,int32_t s32,int64_t s64,float f,double d,const uint8_t *u8_array,const uint16_t *u16_array,const uint32_t *u32_array,const uint64_t *u64_array,const int8_t *s8_array,const int16_t *s16_array,const int32_t *s32_array,const int64_t *s64_array,const float *f_array,const double *d_array) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[179]; - _mav_put_uint64_t(buf, 0, u64); - _mav_put_int64_t(buf, 8, s64); - _mav_put_double(buf, 16, d); - _mav_put_uint32_t(buf, 96, u32); - _mav_put_int32_t(buf, 100, s32); - _mav_put_float(buf, 104, f); - _mav_put_uint16_t(buf, 144, u16); - _mav_put_int16_t(buf, 146, s16); - _mav_put_char(buf, 160, c); - _mav_put_uint8_t(buf, 171, u8); - _mav_put_int8_t(buf, 172, s8); - _mav_put_uint64_t_array(buf, 24, u64_array, 3); - _mav_put_int64_t_array(buf, 48, s64_array, 3); - _mav_put_double_array(buf, 72, d_array, 3); - _mav_put_uint32_t_array(buf, 108, u32_array, 3); - _mav_put_int32_t_array(buf, 120, s32_array, 3); - _mav_put_float_array(buf, 132, f_array, 3); - _mav_put_uint16_t_array(buf, 148, u16_array, 3); - _mav_put_int16_t_array(buf, 154, s16_array, 3); - _mav_put_char_array(buf, 161, s, 10); - _mav_put_uint8_t_array(buf, 173, u8_array, 3); - _mav_put_int8_t_array(buf, 176, s8_array, 3); - memcpy(_MAV_PAYLOAD(msg), buf, 179); -#else - mavlink_test_types_t packet; - packet.u64 = u64; - packet.s64 = s64; - packet.d = d; - packet.u32 = u32; - packet.s32 = s32; - packet.f = f; - packet.u16 = u16; - packet.s16 = s16; - packet.c = c; - packet.u8 = u8; - packet.s8 = s8; - mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3); - mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3); - mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3); - mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3); - mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3); - mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3); - mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3); - mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3); - mav_array_memcpy(packet.s, s, sizeof(char)*10); - mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3); - mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3); - memcpy(_MAV_PAYLOAD(msg), &packet, 179); -#endif - - msg->msgid = MAVLINK_MSG_ID_TEST_TYPES; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 179, 103); -} - -/** - * @brief Encode a test_types struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param test_types C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_test_types_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_test_types_t* test_types) -{ - return mavlink_msg_test_types_pack(system_id, component_id, msg, test_types->c, test_types->s, test_types->u8, test_types->u16, test_types->u32, test_types->u64, test_types->s8, test_types->s16, test_types->s32, test_types->s64, test_types->f, test_types->d, test_types->u8_array, test_types->u16_array, test_types->u32_array, test_types->u64_array, test_types->s8_array, test_types->s16_array, test_types->s32_array, test_types->s64_array, test_types->f_array, test_types->d_array); -} - -/** - * @brief Send a test_types message - * @param chan MAVLink channel to send the message - * - * @param c char - * @param s string - * @param u8 uint8_t - * @param u16 uint16_t - * @param u32 uint32_t - * @param u64 uint64_t - * @param s8 int8_t - * @param s16 int16_t - * @param s32 int32_t - * @param s64 int64_t - * @param f float - * @param d double - * @param u8_array uint8_t_array - * @param u16_array uint16_t_array - * @param u32_array uint32_t_array - * @param u64_array uint64_t_array - * @param s8_array int8_t_array - * @param s16_array int16_t_array - * @param s32_array int32_t_array - * @param s64_array int64_t_array - * @param f_array float_array - * @param d_array double_array - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_test_types_send(mavlink_channel_t chan, char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[179]; - _mav_put_uint64_t(buf, 0, u64); - _mav_put_int64_t(buf, 8, s64); - _mav_put_double(buf, 16, d); - _mav_put_uint32_t(buf, 96, u32); - _mav_put_int32_t(buf, 100, s32); - _mav_put_float(buf, 104, f); - _mav_put_uint16_t(buf, 144, u16); - _mav_put_int16_t(buf, 146, s16); - _mav_put_char(buf, 160, c); - _mav_put_uint8_t(buf, 171, u8); - _mav_put_int8_t(buf, 172, s8); - _mav_put_uint64_t_array(buf, 24, u64_array, 3); - _mav_put_int64_t_array(buf, 48, s64_array, 3); - _mav_put_double_array(buf, 72, d_array, 3); - _mav_put_uint32_t_array(buf, 108, u32_array, 3); - _mav_put_int32_t_array(buf, 120, s32_array, 3); - _mav_put_float_array(buf, 132, f_array, 3); - _mav_put_uint16_t_array(buf, 148, u16_array, 3); - _mav_put_int16_t_array(buf, 154, s16_array, 3); - _mav_put_char_array(buf, 161, s, 10); - _mav_put_uint8_t_array(buf, 173, u8_array, 3); - _mav_put_int8_t_array(buf, 176, s8_array, 3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, buf, 179, 103); -#else - mavlink_test_types_t packet; - packet.u64 = u64; - packet.s64 = s64; - packet.d = d; - packet.u32 = u32; - packet.s32 = s32; - packet.f = f; - packet.u16 = u16; - packet.s16 = s16; - packet.c = c; - packet.u8 = u8; - packet.s8 = s8; - mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3); - mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3); - mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3); - mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3); - mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3); - mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3); - mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3); - mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3); - mav_array_memcpy(packet.s, s, sizeof(char)*10); - mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3); - mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, (const char *)&packet, 179, 103); -#endif -} - -#endif - -// MESSAGE TEST_TYPES UNPACKING - - -/** - * @brief Get field c from test_types message - * - * @return char - */ -static inline char mavlink_msg_test_types_get_c(const mavlink_message_t* msg) -{ - return _MAV_RETURN_char(msg, 160); -} - -/** - * @brief Get field s from test_types message - * - * @return string - */ -static inline uint16_t mavlink_msg_test_types_get_s(const mavlink_message_t* msg, char *s) -{ - return _MAV_RETURN_char_array(msg, s, 10, 161); -} - -/** - * @brief Get field u8 from test_types message - * - * @return uint8_t - */ -static inline uint8_t mavlink_msg_test_types_get_u8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 171); -} - -/** - * @brief Get field u16 from test_types message - * - * @return uint16_t - */ -static inline uint16_t mavlink_msg_test_types_get_u16(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 144); -} - -/** - * @brief Get field u32 from test_types message - * - * @return uint32_t - */ -static inline uint32_t mavlink_msg_test_types_get_u32(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 96); -} - -/** - * @brief Get field u64 from test_types message - * - * @return uint64_t - */ -static inline uint64_t mavlink_msg_test_types_get_u64(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field s8 from test_types message - * - * @return int8_t - */ -static inline int8_t mavlink_msg_test_types_get_s8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int8_t(msg, 172); -} - -/** - * @brief Get field s16 from test_types message - * - * @return int16_t - */ -static inline int16_t mavlink_msg_test_types_get_s16(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 146); -} - -/** - * @brief Get field s32 from test_types message - * - * @return int32_t - */ -static inline int32_t mavlink_msg_test_types_get_s32(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 100); -} - -/** - * @brief Get field s64 from test_types message - * - * @return int64_t - */ -static inline int64_t mavlink_msg_test_types_get_s64(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int64_t(msg, 8); -} - -/** - * @brief Get field f from test_types message - * - * @return float - */ -static inline float mavlink_msg_test_types_get_f(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 104); -} - -/** - * @brief Get field d from test_types message - * - * @return double - */ -static inline double mavlink_msg_test_types_get_d(const mavlink_message_t* msg) -{ - return _MAV_RETURN_double(msg, 16); -} - -/** - * @brief Get field u8_array from test_types message - * - * @return uint8_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_u8_array(const mavlink_message_t* msg, uint8_t *u8_array) -{ - return _MAV_RETURN_uint8_t_array(msg, u8_array, 3, 173); -} - -/** - * @brief Get field u16_array from test_types message - * - * @return uint16_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_u16_array(const mavlink_message_t* msg, uint16_t *u16_array) -{ - return _MAV_RETURN_uint16_t_array(msg, u16_array, 3, 148); -} - -/** - * @brief Get field u32_array from test_types message - * - * @return uint32_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_u32_array(const mavlink_message_t* msg, uint32_t *u32_array) -{ - return _MAV_RETURN_uint32_t_array(msg, u32_array, 3, 108); -} - -/** - * @brief Get field u64_array from test_types message - * - * @return uint64_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_u64_array(const mavlink_message_t* msg, uint64_t *u64_array) -{ - return _MAV_RETURN_uint64_t_array(msg, u64_array, 3, 24); -} - -/** - * @brief Get field s8_array from test_types message - * - * @return int8_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_s8_array(const mavlink_message_t* msg, int8_t *s8_array) -{ - return _MAV_RETURN_int8_t_array(msg, s8_array, 3, 176); -} - -/** - * @brief Get field s16_array from test_types message - * - * @return int16_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_s16_array(const mavlink_message_t* msg, int16_t *s16_array) -{ - return _MAV_RETURN_int16_t_array(msg, s16_array, 3, 154); -} - -/** - * @brief Get field s32_array from test_types message - * - * @return int32_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_s32_array(const mavlink_message_t* msg, int32_t *s32_array) -{ - return _MAV_RETURN_int32_t_array(msg, s32_array, 3, 120); -} - -/** - * @brief Get field s64_array from test_types message - * - * @return int64_t_array - */ -static inline uint16_t mavlink_msg_test_types_get_s64_array(const mavlink_message_t* msg, int64_t *s64_array) -{ - return _MAV_RETURN_int64_t_array(msg, s64_array, 3, 48); -} - -/** - * @brief Get field f_array from test_types message - * - * @return float_array - */ -static inline uint16_t mavlink_msg_test_types_get_f_array(const mavlink_message_t* msg, float *f_array) -{ - return _MAV_RETURN_float_array(msg, f_array, 3, 132); -} - -/** - * @brief Get field d_array from test_types message - * - * @return double_array - */ -static inline uint16_t mavlink_msg_test_types_get_d_array(const mavlink_message_t* msg, double *d_array) -{ - return _MAV_RETURN_double_array(msg, d_array, 3, 72); -} - -/** - * @brief Decode a test_types message into a struct - * - * @param msg The message to decode - * @param test_types C-struct to decode the message contents into - */ -static inline void mavlink_msg_test_types_decode(const mavlink_message_t* msg, mavlink_test_types_t* test_types) -{ -#if MAVLINK_NEED_BYTE_SWAP - test_types->u64 = mavlink_msg_test_types_get_u64(msg); - test_types->s64 = mavlink_msg_test_types_get_s64(msg); - test_types->d = mavlink_msg_test_types_get_d(msg); - mavlink_msg_test_types_get_u64_array(msg, test_types->u64_array); - mavlink_msg_test_types_get_s64_array(msg, test_types->s64_array); - mavlink_msg_test_types_get_d_array(msg, test_types->d_array); - test_types->u32 = mavlink_msg_test_types_get_u32(msg); - test_types->s32 = mavlink_msg_test_types_get_s32(msg); - test_types->f = mavlink_msg_test_types_get_f(msg); - mavlink_msg_test_types_get_u32_array(msg, test_types->u32_array); - mavlink_msg_test_types_get_s32_array(msg, test_types->s32_array); - mavlink_msg_test_types_get_f_array(msg, test_types->f_array); - test_types->u16 = mavlink_msg_test_types_get_u16(msg); - test_types->s16 = mavlink_msg_test_types_get_s16(msg); - mavlink_msg_test_types_get_u16_array(msg, test_types->u16_array); - mavlink_msg_test_types_get_s16_array(msg, test_types->s16_array); - test_types->c = mavlink_msg_test_types_get_c(msg); - mavlink_msg_test_types_get_s(msg, test_types->s); - test_types->u8 = mavlink_msg_test_types_get_u8(msg); - test_types->s8 = mavlink_msg_test_types_get_s8(msg); - mavlink_msg_test_types_get_u8_array(msg, test_types->u8_array); - mavlink_msg_test_types_get_s8_array(msg, test_types->s8_array); -#else - memcpy(test_types, _MAV_PAYLOAD(msg), 179); -#endif -} diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/test.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/test.h deleted file mode 100644 index 4dc04f889..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/test.h +++ /dev/null @@ -1,53 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from test.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef TEST_H -#define TEST_H - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {179, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {103, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_TEST_TYPES, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_TEST - - - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 3 -#endif - -// ENUM DEFINITIONS - - - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_test_types.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // TEST_H diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/testsuite.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/testsuite.h deleted file mode 100644 index 658e1ae07..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/testsuite.h +++ /dev/null @@ -1,120 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from test.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef TEST_TESTSUITE_H -#define TEST_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL - -static void mavlink_test_test(uint8_t, uint8_t, mavlink_message_t *last_msg); - -static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - - mavlink_test_test(system_id, component_id, last_msg); -} -#endif - - - - -static void mavlink_test_test_types(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_test_types_t packet_in = { - 93372036854775807ULL, - 93372036854776311LL, - 235.0, - { 93372036854777319, 93372036854777320, 93372036854777321 }, - { 93372036854778831, 93372036854778832, 93372036854778833 }, - { 627.0, 628.0, 629.0 }, - 963502456, - 963502664, - 745.0, - { 963503080, 963503081, 963503082 }, - { 963503704, 963503705, 963503706 }, - { 941.0, 942.0, 943.0 }, - 24723, - 24827, - { 24931, 24932, 24933 }, - { 25243, 25244, 25245 }, - 'E', - "FGHIJKLMN", - 198, - 9, - { 76, 77, 78 }, - { 21, 22, 23 }, - }; - mavlink_test_types_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.u64 = packet_in.u64; - packet1.s64 = packet_in.s64; - packet1.d = packet_in.d; - packet1.u32 = packet_in.u32; - packet1.s32 = packet_in.s32; - packet1.f = packet_in.f; - packet1.u16 = packet_in.u16; - packet1.s16 = packet_in.s16; - packet1.c = packet_in.c; - packet1.u8 = packet_in.u8; - packet1.s8 = packet_in.s8; - - mav_array_memcpy(packet1.u64_array, packet_in.u64_array, sizeof(uint64_t)*3); - mav_array_memcpy(packet1.s64_array, packet_in.s64_array, sizeof(int64_t)*3); - mav_array_memcpy(packet1.d_array, packet_in.d_array, sizeof(double)*3); - mav_array_memcpy(packet1.u32_array, packet_in.u32_array, sizeof(uint32_t)*3); - mav_array_memcpy(packet1.s32_array, packet_in.s32_array, sizeof(int32_t)*3); - mav_array_memcpy(packet1.f_array, packet_in.f_array, sizeof(float)*3); - mav_array_memcpy(packet1.u16_array, packet_in.u16_array, sizeof(uint16_t)*3); - mav_array_memcpy(packet1.s16_array, packet_in.s16_array, sizeof(int16_t)*3); - mav_array_memcpy(packet1.s, packet_in.s, sizeof(char)*10); - mav_array_memcpy(packet1.u8_array, packet_in.u8_array, sizeof(uint8_t)*3); - mav_array_memcpy(packet1.s8_array, packet_in.s8_array, sizeof(int8_t)*3); - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_test_types_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_test_types_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_test_types_pack(system_id, component_id, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); - mavlink_msg_test_types_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_test_types_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); - mavlink_msg_test_types_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; i - -#include -#include -#include -#include -#include -#include -// @@protoc_insertion_point(includes) - -namespace px { - -namespace { - -const ::google::protobuf::Descriptor* HeaderInfo_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - HeaderInfo_reflection_ = NULL; -const ::google::protobuf::Descriptor* GLOverlay_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - GLOverlay_reflection_ = NULL; -const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor_ = NULL; -const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor_ = NULL; -const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor_ = NULL; -const ::google::protobuf::Descriptor* Obstacle_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - Obstacle_reflection_ = NULL; -const ::google::protobuf::Descriptor* ObstacleList_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - ObstacleList_reflection_ = NULL; -const ::google::protobuf::Descriptor* ObstacleMap_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - ObstacleMap_reflection_ = NULL; -const ::google::protobuf::Descriptor* Path_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - Path_reflection_ = NULL; -const ::google::protobuf::Descriptor* PointCloudXYZI_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - PointCloudXYZI_reflection_ = NULL; -const ::google::protobuf::Descriptor* PointCloudXYZI_PointXYZI_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - PointCloudXYZI_PointXYZI_reflection_ = NULL; -const ::google::protobuf::Descriptor* PointCloudXYZRGB_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - PointCloudXYZRGB_reflection_ = NULL; -const ::google::protobuf::Descriptor* PointCloudXYZRGB_PointXYZRGB_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - PointCloudXYZRGB_PointXYZRGB_reflection_ = NULL; -const ::google::protobuf::Descriptor* RGBDImage_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - RGBDImage_reflection_ = NULL; -const ::google::protobuf::Descriptor* Waypoint_descriptor_ = NULL; -const ::google::protobuf::internal::GeneratedMessageReflection* - Waypoint_reflection_ = NULL; - -} // namespace - - -void protobuf_AssignDesc_pixhawk_2eproto() { - protobuf_AddDesc_pixhawk_2eproto(); - const ::google::protobuf::FileDescriptor* file = - ::google::protobuf::DescriptorPool::generated_pool()->FindFileByName( - "pixhawk.proto"); - GOOGLE_CHECK(file != NULL); - HeaderInfo_descriptor_ = file->message_type(0); - static const int HeaderInfo_offsets_[3] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, source_sysid_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, source_compid_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, timestamp_), - }; - HeaderInfo_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - HeaderInfo_descriptor_, - HeaderInfo::default_instance_, - HeaderInfo_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(HeaderInfo)); - GLOverlay_descriptor_ = file->message_type(1); - static const int GLOverlay_offsets_[7] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, name_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, coordinateframetype_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_x_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_y_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_z_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, data_), - }; - GLOverlay_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - GLOverlay_descriptor_, - GLOverlay::default_instance_, - GLOverlay_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(GLOverlay)); - GLOverlay_CoordinateFrameType_descriptor_ = GLOverlay_descriptor_->enum_type(0); - GLOverlay_Mode_descriptor_ = GLOverlay_descriptor_->enum_type(1); - GLOverlay_Identifier_descriptor_ = GLOverlay_descriptor_->enum_type(2); - Obstacle_descriptor_ = file->message_type(2); - static const int Obstacle_offsets_[6] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, x_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, y_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, z_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, length_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, width_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, height_), - }; - Obstacle_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - Obstacle_descriptor_, - Obstacle::default_instance_, - Obstacle_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(Obstacle)); - ObstacleList_descriptor_ = file->message_type(3); - static const int ObstacleList_offsets_[2] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, obstacles_), - }; - ObstacleList_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - ObstacleList_descriptor_, - ObstacleList::default_instance_, - ObstacleList_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(ObstacleList)); - ObstacleMap_descriptor_ = file->message_type(4); - static const int ObstacleMap_offsets_[10] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, type_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, resolution_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, rows_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, cols_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, mapr0_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, mapc0_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, arrayr0_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, arrayc0_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, data_), - }; - ObstacleMap_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - ObstacleMap_descriptor_, - ObstacleMap::default_instance_, - ObstacleMap_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(ObstacleMap)); - Path_descriptor_ = file->message_type(5); - static const int Path_offsets_[2] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, waypoints_), - }; - Path_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - Path_descriptor_, - Path::default_instance_, - Path_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(Path)); - PointCloudXYZI_descriptor_ = file->message_type(6); - static const int PointCloudXYZI_offsets_[2] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, points_), - }; - PointCloudXYZI_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - PointCloudXYZI_descriptor_, - PointCloudXYZI::default_instance_, - PointCloudXYZI_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(PointCloudXYZI)); - PointCloudXYZI_PointXYZI_descriptor_ = PointCloudXYZI_descriptor_->nested_type(0); - static const int PointCloudXYZI_PointXYZI_offsets_[4] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, x_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, y_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, z_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, intensity_), - }; - PointCloudXYZI_PointXYZI_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - PointCloudXYZI_PointXYZI_descriptor_, - PointCloudXYZI_PointXYZI::default_instance_, - PointCloudXYZI_PointXYZI_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(PointCloudXYZI_PointXYZI)); - PointCloudXYZRGB_descriptor_ = file->message_type(7); - static const int PointCloudXYZRGB_offsets_[2] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, points_), - }; - PointCloudXYZRGB_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - PointCloudXYZRGB_descriptor_, - PointCloudXYZRGB::default_instance_, - PointCloudXYZRGB_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(PointCloudXYZRGB)); - PointCloudXYZRGB_PointXYZRGB_descriptor_ = PointCloudXYZRGB_descriptor_->nested_type(0); - static const int PointCloudXYZRGB_PointXYZRGB_offsets_[4] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, x_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, y_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, z_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, rgb_), - }; - PointCloudXYZRGB_PointXYZRGB_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - PointCloudXYZRGB_PointXYZRGB_descriptor_, - PointCloudXYZRGB_PointXYZRGB::default_instance_, - PointCloudXYZRGB_PointXYZRGB_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(PointCloudXYZRGB_PointXYZRGB)); - RGBDImage_descriptor_ = file->message_type(8); - static const int RGBDImage_offsets_[21] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, header_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, cols_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, rows_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, step1_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, type1_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, imagedata1_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, step2_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, type2_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, imagedata2_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, camera_config_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, camera_type_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, roll_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, pitch_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, yaw_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, lon_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, lat_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, alt_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, ground_x_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, ground_y_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, ground_z_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, camera_matrix_), - }; - RGBDImage_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - RGBDImage_descriptor_, - RGBDImage::default_instance_, - RGBDImage_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(RGBDImage)); - Waypoint_descriptor_ = file->message_type(9); - static const int Waypoint_offsets_[6] = { - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, x_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, y_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, z_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, roll_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, pitch_), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, yaw_), - }; - Waypoint_reflection_ = - new ::google::protobuf::internal::GeneratedMessageReflection( - Waypoint_descriptor_, - Waypoint::default_instance_, - Waypoint_offsets_, - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, _has_bits_[0]), - GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, _unknown_fields_), - -1, - ::google::protobuf::DescriptorPool::generated_pool(), - ::google::protobuf::MessageFactory::generated_factory(), - sizeof(Waypoint)); -} - -namespace { - -GOOGLE_PROTOBUF_DECLARE_ONCE(protobuf_AssignDescriptors_once_); -inline void protobuf_AssignDescriptorsOnce() { - ::google::protobuf::GoogleOnceInit(&protobuf_AssignDescriptors_once_, - &protobuf_AssignDesc_pixhawk_2eproto); -} - -void protobuf_RegisterTypes(const ::std::string&) { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - HeaderInfo_descriptor_, &HeaderInfo::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - GLOverlay_descriptor_, &GLOverlay::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - Obstacle_descriptor_, &Obstacle::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - ObstacleList_descriptor_, &ObstacleList::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - ObstacleMap_descriptor_, &ObstacleMap::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - Path_descriptor_, &Path::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - PointCloudXYZI_descriptor_, &PointCloudXYZI::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - PointCloudXYZI_PointXYZI_descriptor_, &PointCloudXYZI_PointXYZI::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - PointCloudXYZRGB_descriptor_, &PointCloudXYZRGB::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - PointCloudXYZRGB_PointXYZRGB_descriptor_, &PointCloudXYZRGB_PointXYZRGB::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - RGBDImage_descriptor_, &RGBDImage::default_instance()); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage( - Waypoint_descriptor_, &Waypoint::default_instance()); -} - -} // namespace - -void protobuf_ShutdownFile_pixhawk_2eproto() { - delete HeaderInfo::default_instance_; - delete HeaderInfo_reflection_; - delete GLOverlay::default_instance_; - delete GLOverlay_reflection_; - delete Obstacle::default_instance_; - delete Obstacle_reflection_; - delete ObstacleList::default_instance_; - delete ObstacleList_reflection_; - delete ObstacleMap::default_instance_; - delete ObstacleMap_reflection_; - delete Path::default_instance_; - delete Path_reflection_; - delete PointCloudXYZI::default_instance_; - delete PointCloudXYZI_reflection_; - delete PointCloudXYZI_PointXYZI::default_instance_; - delete PointCloudXYZI_PointXYZI_reflection_; - delete PointCloudXYZRGB::default_instance_; - delete PointCloudXYZRGB_reflection_; - delete PointCloudXYZRGB_PointXYZRGB::default_instance_; - delete PointCloudXYZRGB_PointXYZRGB_reflection_; - delete RGBDImage::default_instance_; - delete RGBDImage_reflection_; - delete Waypoint::default_instance_; - delete Waypoint_reflection_; -} - -void protobuf_AddDesc_pixhawk_2eproto() { - static bool already_here = false; - if (already_here) return; - already_here = true; - GOOGLE_PROTOBUF_VERIFY_VERSION; - - ::google::protobuf::DescriptorPool::InternalAddGeneratedFile( - "\n\rpixhawk.proto\022\002px\"L\n\nHeaderInfo\022\024\n\014sou" - "rce_sysid\030\001 \002(\005\022\025\n\rsource_compid\030\002 \002(\005\022\021" - "\n\ttimestamp\030\003 \002(\001\"\377\004\n\tGLOverlay\022\036\n\006heade" - "r\030\001 \002(\0132\016.px.HeaderInfo\022\014\n\004name\030\002 \001(\t\022>\n" - "\023coordinateFrameType\030\003 \001(\0162!.px.GLOverla" - "y.CoordinateFrameType\022\020\n\010origin_x\030\004 \001(\001\022" - "\020\n\010origin_y\030\005 \001(\001\022\020\n\010origin_z\030\006 \001(\001\022\014\n\004d" - "ata\030\007 \001(\014\",\n\023CoordinateFrameType\022\n\n\006GLOB" - "AL\020\000\022\t\n\005LOCAL\020\001\"\333\001\n\004Mode\022\n\n\006POINTS\020\000\022\t\n\005" - "LINES\020\001\022\016\n\nLINE_STRIP\020\002\022\r\n\tLINE_LOOP\020\003\022\r" - "\n\tTRIANGLES\020\004\022\022\n\016TRIANGLE_STRIP\020\005\022\020\n\014TRI" - "ANGLE_FAN\020\006\022\t\n\005QUADS\020\007\022\016\n\nQUAD_STRIP\020\010\022\013" - "\n\007POLYGON\020\t\022\020\n\014SOLID_CIRCLE\020\n\022\017\n\013WIRE_CI" - "RCLE\020\013\022\016\n\nSOLID_CUBE\020\014\022\r\n\tWIRE_CUBE\020\r\"\263\001" - "\n\nIdentifier\022\007\n\003END\020\016\022\014\n\010VERTEX2F\020\017\022\014\n\010V" - "ERTEX3F\020\020\022\013\n\007ROTATEF\020\021\022\016\n\nTRANSLATEF\020\022\022\n" - "\n\006SCALEF\020\023\022\017\n\013PUSH_MATRIX\020\024\022\016\n\nPOP_MATRI" - "X\020\025\022\013\n\007COLOR3F\020\026\022\013\n\007COLOR4F\020\027\022\r\n\tPOINTSI" - "ZE\020\030\022\r\n\tLINEWIDTH\020\031\"Z\n\010Obstacle\022\t\n\001x\030\001 \001" - "(\002\022\t\n\001y\030\002 \001(\002\022\t\n\001z\030\003 \001(\002\022\016\n\006length\030\004 \001(\002" - "\022\r\n\005width\030\005 \001(\002\022\016\n\006height\030\006 \001(\002\"O\n\014Obsta" - "cleList\022\036\n\006header\030\001 \002(\0132\016.px.HeaderInfo\022" - "\037\n\tobstacles\030\002 \003(\0132\014.px.Obstacle\"\271\001\n\013Obs" - "tacleMap\022\036\n\006header\030\001 \002(\0132\016.px.HeaderInfo" - "\022\014\n\004type\030\002 \002(\005\022\022\n\nresolution\030\003 \001(\002\022\014\n\004ro" - "ws\030\004 \001(\005\022\014\n\004cols\030\005 \001(\005\022\r\n\005mapR0\030\006 \001(\005\022\r\n" - "\005mapC0\030\007 \001(\005\022\017\n\007arrayR0\030\010 \001(\005\022\017\n\007arrayC0" - "\030\t \001(\005\022\014\n\004data\030\n \001(\014\"G\n\004Path\022\036\n\006header\030\001" - " \002(\0132\016.px.HeaderInfo\022\037\n\twaypoints\030\002 \003(\0132" - "\014.px.Waypoint\"\237\001\n\016PointCloudXYZI\022\036\n\006head" - "er\030\001 \002(\0132\016.px.HeaderInfo\022,\n\006points\030\002 \003(\013" - "2\034.px.PointCloudXYZI.PointXYZI\032\?\n\tPointX" - "YZI\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z\030\003 \002(\002\022\021\n\t" - "intensity\030\004 \002(\002\"\241\001\n\020PointCloudXYZRGB\022\036\n\006" - "header\030\001 \002(\0132\016.px.HeaderInfo\0220\n\006points\030\002" - " \003(\0132 .px.PointCloudXYZRGB.PointXYZRGB\032;" - "\n\013PointXYZRGB\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z" - "\030\003 \002(\002\022\013\n\003rgb\030\004 \002(\002\"\365\002\n\tRGBDImage\022\036\n\006hea" - "der\030\001 \002(\0132\016.px.HeaderInfo\022\014\n\004cols\030\002 \002(\r\022" - "\014\n\004rows\030\003 \002(\r\022\r\n\005step1\030\004 \002(\r\022\r\n\005type1\030\005 " - "\002(\r\022\022\n\nimageData1\030\006 \002(\014\022\r\n\005step2\030\007 \002(\r\022\r" - "\n\005type2\030\010 \002(\r\022\022\n\nimageData2\030\t \002(\014\022\025\n\rcam" - "era_config\030\n \001(\r\022\023\n\013camera_type\030\013 \001(\r\022\014\n" - "\004roll\030\014 \001(\002\022\r\n\005pitch\030\r \001(\002\022\013\n\003yaw\030\016 \001(\002\022" - "\013\n\003lon\030\017 \001(\002\022\013\n\003lat\030\020 \001(\002\022\013\n\003alt\030\021 \001(\002\022\020" - "\n\010ground_x\030\022 \001(\002\022\020\n\010ground_y\030\023 \001(\002\022\020\n\010gr" - "ound_z\030\024 \001(\002\022\025\n\rcamera_matrix\030\025 \003(\002\"U\n\010W" - "aypoint\022\t\n\001x\030\001 \002(\001\022\t\n\001y\030\002 \002(\001\022\t\n\001z\030\003 \001(\001" - "\022\014\n\004roll\030\004 \001(\001\022\r\n\005pitch\030\005 \001(\001\022\013\n\003yaw\030\006 \001" - "(\001", 1962); - ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile( - "pixhawk.proto", &protobuf_RegisterTypes); - HeaderInfo::default_instance_ = new HeaderInfo(); - GLOverlay::default_instance_ = new GLOverlay(); - Obstacle::default_instance_ = new Obstacle(); - ObstacleList::default_instance_ = new ObstacleList(); - ObstacleMap::default_instance_ = new ObstacleMap(); - Path::default_instance_ = new Path(); - PointCloudXYZI::default_instance_ = new PointCloudXYZI(); - PointCloudXYZI_PointXYZI::default_instance_ = new PointCloudXYZI_PointXYZI(); - PointCloudXYZRGB::default_instance_ = new PointCloudXYZRGB(); - PointCloudXYZRGB_PointXYZRGB::default_instance_ = new PointCloudXYZRGB_PointXYZRGB(); - RGBDImage::default_instance_ = new RGBDImage(); - Waypoint::default_instance_ = new Waypoint(); - HeaderInfo::default_instance_->InitAsDefaultInstance(); - GLOverlay::default_instance_->InitAsDefaultInstance(); - Obstacle::default_instance_->InitAsDefaultInstance(); - ObstacleList::default_instance_->InitAsDefaultInstance(); - ObstacleMap::default_instance_->InitAsDefaultInstance(); - Path::default_instance_->InitAsDefaultInstance(); - PointCloudXYZI::default_instance_->InitAsDefaultInstance(); - PointCloudXYZI_PointXYZI::default_instance_->InitAsDefaultInstance(); - PointCloudXYZRGB::default_instance_->InitAsDefaultInstance(); - PointCloudXYZRGB_PointXYZRGB::default_instance_->InitAsDefaultInstance(); - RGBDImage::default_instance_->InitAsDefaultInstance(); - Waypoint::default_instance_->InitAsDefaultInstance(); - ::google::protobuf::internal::OnShutdown(&protobuf_ShutdownFile_pixhawk_2eproto); -} - -// Force AddDescriptors() to be called at static initialization time. -struct StaticDescriptorInitializer_pixhawk_2eproto { - StaticDescriptorInitializer_pixhawk_2eproto() { - protobuf_AddDesc_pixhawk_2eproto(); - } -} static_descriptor_initializer_pixhawk_2eproto_; - - -// =================================================================== - -#ifndef _MSC_VER -const int HeaderInfo::kSourceSysidFieldNumber; -const int HeaderInfo::kSourceCompidFieldNumber; -const int HeaderInfo::kTimestampFieldNumber; -#endif // !_MSC_VER - -HeaderInfo::HeaderInfo() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void HeaderInfo::InitAsDefaultInstance() { -} - -HeaderInfo::HeaderInfo(const HeaderInfo& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void HeaderInfo::SharedCtor() { - _cached_size_ = 0; - source_sysid_ = 0; - source_compid_ = 0; - timestamp_ = 0; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -HeaderInfo::~HeaderInfo() { - SharedDtor(); -} - -void HeaderInfo::SharedDtor() { - if (this != default_instance_) { - } -} - -void HeaderInfo::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* HeaderInfo::descriptor() { - protobuf_AssignDescriptorsOnce(); - return HeaderInfo_descriptor_; -} - -const HeaderInfo& HeaderInfo::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -HeaderInfo* HeaderInfo::default_instance_ = NULL; - -HeaderInfo* HeaderInfo::New() const { - return new HeaderInfo; -} - -void HeaderInfo::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - source_sysid_ = 0; - source_compid_ = 0; - timestamp_ = 0; - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool HeaderInfo::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required int32 source_sysid = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &source_sysid_))); - set_has_source_sysid(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(16)) goto parse_source_compid; - break; - } - - // required int32 source_compid = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_source_compid: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &source_compid_))); - set_has_source_compid(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(25)) goto parse_timestamp; - break; - } - - // required double timestamp = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_timestamp: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, ×tamp_))); - set_has_timestamp(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void HeaderInfo::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required int32 source_sysid = 1; - if (has_source_sysid()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(1, this->source_sysid(), output); - } - - // required int32 source_compid = 2; - if (has_source_compid()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(2, this->source_compid(), output); - } - - // required double timestamp = 3; - if (has_timestamp()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(3, this->timestamp(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* HeaderInfo::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required int32 source_sysid = 1; - if (has_source_sysid()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(1, this->source_sysid(), target); - } - - // required int32 source_compid = 2; - if (has_source_compid()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(2, this->source_compid(), target); - } - - // required double timestamp = 3; - if (has_timestamp()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(3, this->timestamp(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int HeaderInfo::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required int32 source_sysid = 1; - if (has_source_sysid()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->source_sysid()); - } - - // required int32 source_compid = 2; - if (has_source_compid()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->source_compid()); - } - - // required double timestamp = 3; - if (has_timestamp()) { - total_size += 1 + 8; - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void HeaderInfo::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const HeaderInfo* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void HeaderInfo::MergeFrom(const HeaderInfo& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_source_sysid()) { - set_source_sysid(from.source_sysid()); - } - if (from.has_source_compid()) { - set_source_compid(from.source_compid()); - } - if (from.has_timestamp()) { - set_timestamp(from.timestamp()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void HeaderInfo::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void HeaderInfo::CopyFrom(const HeaderInfo& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool HeaderInfo::IsInitialized() const { - if ((_has_bits_[0] & 0x00000007) != 0x00000007) return false; - - return true; -} - -void HeaderInfo::Swap(HeaderInfo* other) { - if (other != this) { - std::swap(source_sysid_, other->source_sysid_); - std::swap(source_compid_, other->source_compid_); - std::swap(timestamp_, other->timestamp_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata HeaderInfo::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = HeaderInfo_descriptor_; - metadata.reflection = HeaderInfo_reflection_; - return metadata; -} - - -// =================================================================== - -const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor() { - protobuf_AssignDescriptorsOnce(); - return GLOverlay_CoordinateFrameType_descriptor_; -} -bool GLOverlay_CoordinateFrameType_IsValid(int value) { - switch(value) { - case 0: - case 1: - return true; - default: - return false; - } -} - -#ifndef _MSC_VER -const GLOverlay_CoordinateFrameType GLOverlay::GLOBAL; -const GLOverlay_CoordinateFrameType GLOverlay::LOCAL; -const GLOverlay_CoordinateFrameType GLOverlay::CoordinateFrameType_MIN; -const GLOverlay_CoordinateFrameType GLOverlay::CoordinateFrameType_MAX; -const int GLOverlay::CoordinateFrameType_ARRAYSIZE; -#endif // _MSC_VER -const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor() { - protobuf_AssignDescriptorsOnce(); - return GLOverlay_Mode_descriptor_; -} -bool GLOverlay_Mode_IsValid(int value) { - switch(value) { - case 0: - case 1: - case 2: - case 3: - case 4: - case 5: - case 6: - case 7: - case 8: - case 9: - case 10: - case 11: - case 12: - case 13: - return true; - default: - return false; - } -} - -#ifndef _MSC_VER -const GLOverlay_Mode GLOverlay::POINTS; -const GLOverlay_Mode GLOverlay::LINES; -const GLOverlay_Mode GLOverlay::LINE_STRIP; -const GLOverlay_Mode GLOverlay::LINE_LOOP; -const GLOverlay_Mode GLOverlay::TRIANGLES; -const GLOverlay_Mode GLOverlay::TRIANGLE_STRIP; -const GLOverlay_Mode GLOverlay::TRIANGLE_FAN; -const GLOverlay_Mode GLOverlay::QUADS; -const GLOverlay_Mode GLOverlay::QUAD_STRIP; -const GLOverlay_Mode GLOverlay::POLYGON; -const GLOverlay_Mode GLOverlay::SOLID_CIRCLE; -const GLOverlay_Mode GLOverlay::WIRE_CIRCLE; -const GLOverlay_Mode GLOverlay::SOLID_CUBE; -const GLOverlay_Mode GLOverlay::WIRE_CUBE; -const GLOverlay_Mode GLOverlay::Mode_MIN; -const GLOverlay_Mode GLOverlay::Mode_MAX; -const int GLOverlay::Mode_ARRAYSIZE; -#endif // _MSC_VER -const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor() { - protobuf_AssignDescriptorsOnce(); - return GLOverlay_Identifier_descriptor_; -} -bool GLOverlay_Identifier_IsValid(int value) { - switch(value) { - case 14: - case 15: - case 16: - case 17: - case 18: - case 19: - case 20: - case 21: - case 22: - case 23: - case 24: - case 25: - return true; - default: - return false; - } -} - -#ifndef _MSC_VER -const GLOverlay_Identifier GLOverlay::END; -const GLOverlay_Identifier GLOverlay::VERTEX2F; -const GLOverlay_Identifier GLOverlay::VERTEX3F; -const GLOverlay_Identifier GLOverlay::ROTATEF; -const GLOverlay_Identifier GLOverlay::TRANSLATEF; -const GLOverlay_Identifier GLOverlay::SCALEF; -const GLOverlay_Identifier GLOverlay::PUSH_MATRIX; -const GLOverlay_Identifier GLOverlay::POP_MATRIX; -const GLOverlay_Identifier GLOverlay::COLOR3F; -const GLOverlay_Identifier GLOverlay::COLOR4F; -const GLOverlay_Identifier GLOverlay::POINTSIZE; -const GLOverlay_Identifier GLOverlay::LINEWIDTH; -const GLOverlay_Identifier GLOverlay::Identifier_MIN; -const GLOverlay_Identifier GLOverlay::Identifier_MAX; -const int GLOverlay::Identifier_ARRAYSIZE; -#endif // _MSC_VER -#ifndef _MSC_VER -const int GLOverlay::kHeaderFieldNumber; -const int GLOverlay::kNameFieldNumber; -const int GLOverlay::kCoordinateFrameTypeFieldNumber; -const int GLOverlay::kOriginXFieldNumber; -const int GLOverlay::kOriginYFieldNumber; -const int GLOverlay::kOriginZFieldNumber; -const int GLOverlay::kDataFieldNumber; -#endif // !_MSC_VER - -GLOverlay::GLOverlay() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void GLOverlay::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -GLOverlay::GLOverlay(const GLOverlay& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void GLOverlay::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - name_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - coordinateframetype_ = 0; - origin_x_ = 0; - origin_y_ = 0; - origin_z_ = 0; - data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -GLOverlay::~GLOverlay() { - SharedDtor(); -} - -void GLOverlay::SharedDtor() { - if (name_ != &::google::protobuf::internal::kEmptyString) { - delete name_; - } - if (data_ != &::google::protobuf::internal::kEmptyString) { - delete data_; - } - if (this != default_instance_) { - delete header_; - } -} - -void GLOverlay::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* GLOverlay::descriptor() { - protobuf_AssignDescriptorsOnce(); - return GLOverlay_descriptor_; -} - -const GLOverlay& GLOverlay::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -GLOverlay* GLOverlay::default_instance_ = NULL; - -GLOverlay* GLOverlay::New() const { - return new GLOverlay; -} - -void GLOverlay::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - if (has_name()) { - if (name_ != &::google::protobuf::internal::kEmptyString) { - name_->clear(); - } - } - coordinateframetype_ = 0; - origin_x_ = 0; - origin_y_ = 0; - origin_z_ = 0; - if (has_data()) { - if (data_ != &::google::protobuf::internal::kEmptyString) { - data_->clear(); - } - } - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool GLOverlay::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_name; - break; - } - - // optional string name = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_name: - DO_(::google::protobuf::internal::WireFormatLite::ReadString( - input, this->mutable_name())); - ::google::protobuf::internal::WireFormat::VerifyUTF8String( - this->name().data(), this->name().length(), - ::google::protobuf::internal::WireFormat::PARSE); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(24)) goto parse_coordinateFrameType; - break; - } - - // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_coordinateFrameType: - int value; - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - int, ::google::protobuf::internal::WireFormatLite::TYPE_ENUM>( - input, &value))); - if (::px::GLOverlay_CoordinateFrameType_IsValid(value)) { - set_coordinateframetype(static_cast< ::px::GLOverlay_CoordinateFrameType >(value)); - } else { - mutable_unknown_fields()->AddVarint(3, value); - } - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(33)) goto parse_origin_x; - break; - } - - // optional double origin_x = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_origin_x: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &origin_x_))); - set_has_origin_x(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(41)) goto parse_origin_y; - break; - } - - // optional double origin_y = 5; - case 5: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_origin_y: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &origin_y_))); - set_has_origin_y(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(49)) goto parse_origin_z; - break; - } - - // optional double origin_z = 6; - case 6: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_origin_z: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &origin_z_))); - set_has_origin_z(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(58)) goto parse_data; - break; - } - - // optional bytes data = 7; - case 7: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_data: - DO_(::google::protobuf::internal::WireFormatLite::ReadBytes( - input, this->mutable_data())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void GLOverlay::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // optional string name = 2; - if (has_name()) { - ::google::protobuf::internal::WireFormat::VerifyUTF8String( - this->name().data(), this->name().length(), - ::google::protobuf::internal::WireFormat::SERIALIZE); - ::google::protobuf::internal::WireFormatLite::WriteString( - 2, this->name(), output); - } - - // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; - if (has_coordinateframetype()) { - ::google::protobuf::internal::WireFormatLite::WriteEnum( - 3, this->coordinateframetype(), output); - } - - // optional double origin_x = 4; - if (has_origin_x()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(4, this->origin_x(), output); - } - - // optional double origin_y = 5; - if (has_origin_y()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(5, this->origin_y(), output); - } - - // optional double origin_z = 6; - if (has_origin_z()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(6, this->origin_z(), output); - } - - // optional bytes data = 7; - if (has_data()) { - ::google::protobuf::internal::WireFormatLite::WriteBytes( - 7, this->data(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* GLOverlay::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // optional string name = 2; - if (has_name()) { - ::google::protobuf::internal::WireFormat::VerifyUTF8String( - this->name().data(), this->name().length(), - ::google::protobuf::internal::WireFormat::SERIALIZE); - target = - ::google::protobuf::internal::WireFormatLite::WriteStringToArray( - 2, this->name(), target); - } - - // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; - if (has_coordinateframetype()) { - target = ::google::protobuf::internal::WireFormatLite::WriteEnumToArray( - 3, this->coordinateframetype(), target); - } - - // optional double origin_x = 4; - if (has_origin_x()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(4, this->origin_x(), target); - } - - // optional double origin_y = 5; - if (has_origin_y()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(5, this->origin_y(), target); - } - - // optional double origin_z = 6; - if (has_origin_z()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(6, this->origin_z(), target); - } - - // optional bytes data = 7; - if (has_data()) { - target = - ::google::protobuf::internal::WireFormatLite::WriteBytesToArray( - 7, this->data(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int GLOverlay::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - // optional string name = 2; - if (has_name()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::StringSize( - this->name()); - } - - // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3; - if (has_coordinateframetype()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::EnumSize(this->coordinateframetype()); - } - - // optional double origin_x = 4; - if (has_origin_x()) { - total_size += 1 + 8; - } - - // optional double origin_y = 5; - if (has_origin_y()) { - total_size += 1 + 8; - } - - // optional double origin_z = 6; - if (has_origin_z()) { - total_size += 1 + 8; - } - - // optional bytes data = 7; - if (has_data()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::BytesSize( - this->data()); - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void GLOverlay::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const GLOverlay* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void GLOverlay::MergeFrom(const GLOverlay& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - if (from.has_name()) { - set_name(from.name()); - } - if (from.has_coordinateframetype()) { - set_coordinateframetype(from.coordinateframetype()); - } - if (from.has_origin_x()) { - set_origin_x(from.origin_x()); - } - if (from.has_origin_y()) { - set_origin_y(from.origin_y()); - } - if (from.has_origin_z()) { - set_origin_z(from.origin_z()); - } - if (from.has_data()) { - set_data(from.data()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void GLOverlay::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void GLOverlay::CopyFrom(const GLOverlay& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool GLOverlay::IsInitialized() const { - if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - return true; -} - -void GLOverlay::Swap(GLOverlay* other) { - if (other != this) { - std::swap(header_, other->header_); - std::swap(name_, other->name_); - std::swap(coordinateframetype_, other->coordinateframetype_); - std::swap(origin_x_, other->origin_x_); - std::swap(origin_y_, other->origin_y_); - std::swap(origin_z_, other->origin_z_); - std::swap(data_, other->data_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata GLOverlay::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = GLOverlay_descriptor_; - metadata.reflection = GLOverlay_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int Obstacle::kXFieldNumber; -const int Obstacle::kYFieldNumber; -const int Obstacle::kZFieldNumber; -const int Obstacle::kLengthFieldNumber; -const int Obstacle::kWidthFieldNumber; -const int Obstacle::kHeightFieldNumber; -#endif // !_MSC_VER - -Obstacle::Obstacle() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void Obstacle::InitAsDefaultInstance() { -} - -Obstacle::Obstacle(const Obstacle& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void Obstacle::SharedCtor() { - _cached_size_ = 0; - x_ = 0; - y_ = 0; - z_ = 0; - length_ = 0; - width_ = 0; - height_ = 0; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -Obstacle::~Obstacle() { - SharedDtor(); -} - -void Obstacle::SharedDtor() { - if (this != default_instance_) { - } -} - -void Obstacle::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* Obstacle::descriptor() { - protobuf_AssignDescriptorsOnce(); - return Obstacle_descriptor_; -} - -const Obstacle& Obstacle::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -Obstacle* Obstacle::default_instance_ = NULL; - -Obstacle* Obstacle::New() const { - return new Obstacle; -} - -void Obstacle::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - x_ = 0; - y_ = 0; - z_ = 0; - length_ = 0; - width_ = 0; - height_ = 0; - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool Obstacle::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // optional float x = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &x_))); - set_has_x(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(21)) goto parse_y; - break; - } - - // optional float y = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_y: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &y_))); - set_has_y(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(29)) goto parse_z; - break; - } - - // optional float z = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_z: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &z_))); - set_has_z(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(37)) goto parse_length; - break; - } - - // optional float length = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_length: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &length_))); - set_has_length(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(45)) goto parse_width; - break; - } - - // optional float width = 5; - case 5: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_width: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &width_))); - set_has_width(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(53)) goto parse_height; - break; - } - - // optional float height = 6; - case 6: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_height: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &height_))); - set_has_height(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void Obstacle::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // optional float x = 1; - if (has_x()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output); - } - - // optional float y = 2; - if (has_y()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output); - } - - // optional float z = 3; - if (has_z()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output); - } - - // optional float length = 4; - if (has_length()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->length(), output); - } - - // optional float width = 5; - if (has_width()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(5, this->width(), output); - } - - // optional float height = 6; - if (has_height()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(6, this->height(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* Obstacle::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // optional float x = 1; - if (has_x()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target); - } - - // optional float y = 2; - if (has_y()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target); - } - - // optional float z = 3; - if (has_z()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target); - } - - // optional float length = 4; - if (has_length()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->length(), target); - } - - // optional float width = 5; - if (has_width()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(5, this->width(), target); - } - - // optional float height = 6; - if (has_height()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(6, this->height(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int Obstacle::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // optional float x = 1; - if (has_x()) { - total_size += 1 + 4; - } - - // optional float y = 2; - if (has_y()) { - total_size += 1 + 4; - } - - // optional float z = 3; - if (has_z()) { - total_size += 1 + 4; - } - - // optional float length = 4; - if (has_length()) { - total_size += 1 + 4; - } - - // optional float width = 5; - if (has_width()) { - total_size += 1 + 4; - } - - // optional float height = 6; - if (has_height()) { - total_size += 1 + 4; - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void Obstacle::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const Obstacle* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void Obstacle::MergeFrom(const Obstacle& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_x()) { - set_x(from.x()); - } - if (from.has_y()) { - set_y(from.y()); - } - if (from.has_z()) { - set_z(from.z()); - } - if (from.has_length()) { - set_length(from.length()); - } - if (from.has_width()) { - set_width(from.width()); - } - if (from.has_height()) { - set_height(from.height()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void Obstacle::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void Obstacle::CopyFrom(const Obstacle& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool Obstacle::IsInitialized() const { - - return true; -} - -void Obstacle::Swap(Obstacle* other) { - if (other != this) { - std::swap(x_, other->x_); - std::swap(y_, other->y_); - std::swap(z_, other->z_); - std::swap(length_, other->length_); - std::swap(width_, other->width_); - std::swap(height_, other->height_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata Obstacle::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = Obstacle_descriptor_; - metadata.reflection = Obstacle_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int ObstacleList::kHeaderFieldNumber; -const int ObstacleList::kObstaclesFieldNumber; -#endif // !_MSC_VER - -ObstacleList::ObstacleList() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void ObstacleList::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -ObstacleList::ObstacleList(const ObstacleList& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void ObstacleList::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -ObstacleList::~ObstacleList() { - SharedDtor(); -} - -void ObstacleList::SharedDtor() { - if (this != default_instance_) { - delete header_; - } -} - -void ObstacleList::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* ObstacleList::descriptor() { - protobuf_AssignDescriptorsOnce(); - return ObstacleList_descriptor_; -} - -const ObstacleList& ObstacleList::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -ObstacleList* ObstacleList::default_instance_ = NULL; - -ObstacleList* ObstacleList::New() const { - return new ObstacleList; -} - -void ObstacleList::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - } - obstacles_.Clear(); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool ObstacleList::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_obstacles; - break; - } - - // repeated .px.Obstacle obstacles = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_obstacles: - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, add_obstacles())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_obstacles; - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void ObstacleList::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // repeated .px.Obstacle obstacles = 2; - for (int i = 0; i < this->obstacles_size(); i++) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 2, this->obstacles(i), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* ObstacleList::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // repeated .px.Obstacle obstacles = 2; - for (int i = 0; i < this->obstacles_size(); i++) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 2, this->obstacles(i), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int ObstacleList::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - } - // repeated .px.Obstacle obstacles = 2; - total_size += 1 * this->obstacles_size(); - for (int i = 0; i < this->obstacles_size(); i++) { - total_size += - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->obstacles(i)); - } - - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void ObstacleList::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const ObstacleList* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void ObstacleList::MergeFrom(const ObstacleList& from) { - GOOGLE_CHECK_NE(&from, this); - obstacles_.MergeFrom(from.obstacles_); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void ObstacleList::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void ObstacleList::CopyFrom(const ObstacleList& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool ObstacleList::IsInitialized() const { - if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - return true; -} - -void ObstacleList::Swap(ObstacleList* other) { - if (other != this) { - std::swap(header_, other->header_); - obstacles_.Swap(&other->obstacles_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata ObstacleList::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = ObstacleList_descriptor_; - metadata.reflection = ObstacleList_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int ObstacleMap::kHeaderFieldNumber; -const int ObstacleMap::kTypeFieldNumber; -const int ObstacleMap::kResolutionFieldNumber; -const int ObstacleMap::kRowsFieldNumber; -const int ObstacleMap::kColsFieldNumber; -const int ObstacleMap::kMapR0FieldNumber; -const int ObstacleMap::kMapC0FieldNumber; -const int ObstacleMap::kArrayR0FieldNumber; -const int ObstacleMap::kArrayC0FieldNumber; -const int ObstacleMap::kDataFieldNumber; -#endif // !_MSC_VER - -ObstacleMap::ObstacleMap() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void ObstacleMap::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -ObstacleMap::ObstacleMap(const ObstacleMap& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void ObstacleMap::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - type_ = 0; - resolution_ = 0; - rows_ = 0; - cols_ = 0; - mapr0_ = 0; - mapc0_ = 0; - arrayr0_ = 0; - arrayc0_ = 0; - data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -ObstacleMap::~ObstacleMap() { - SharedDtor(); -} - -void ObstacleMap::SharedDtor() { - if (data_ != &::google::protobuf::internal::kEmptyString) { - delete data_; - } - if (this != default_instance_) { - delete header_; - } -} - -void ObstacleMap::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* ObstacleMap::descriptor() { - protobuf_AssignDescriptorsOnce(); - return ObstacleMap_descriptor_; -} - -const ObstacleMap& ObstacleMap::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -ObstacleMap* ObstacleMap::default_instance_ = NULL; - -ObstacleMap* ObstacleMap::New() const { - return new ObstacleMap; -} - -void ObstacleMap::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - type_ = 0; - resolution_ = 0; - rows_ = 0; - cols_ = 0; - mapr0_ = 0; - mapc0_ = 0; - arrayr0_ = 0; - } - if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) { - arrayc0_ = 0; - if (has_data()) { - if (data_ != &::google::protobuf::internal::kEmptyString) { - data_->clear(); - } - } - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool ObstacleMap::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(16)) goto parse_type; - break; - } - - // required int32 type = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_type: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &type_))); - set_has_type(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(29)) goto parse_resolution; - break; - } - - // optional float resolution = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_resolution: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &resolution_))); - set_has_resolution(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(32)) goto parse_rows; - break; - } - - // optional int32 rows = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_rows: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &rows_))); - set_has_rows(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(40)) goto parse_cols; - break; - } - - // optional int32 cols = 5; - case 5: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_cols: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &cols_))); - set_has_cols(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(48)) goto parse_mapR0; - break; - } - - // optional int32 mapR0 = 6; - case 6: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_mapR0: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &mapr0_))); - set_has_mapr0(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(56)) goto parse_mapC0; - break; - } - - // optional int32 mapC0 = 7; - case 7: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_mapC0: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &mapc0_))); - set_has_mapc0(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(64)) goto parse_arrayR0; - break; - } - - // optional int32 arrayR0 = 8; - case 8: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_arrayR0: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &arrayr0_))); - set_has_arrayr0(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(72)) goto parse_arrayC0; - break; - } - - // optional int32 arrayC0 = 9; - case 9: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_arrayC0: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>( - input, &arrayc0_))); - set_has_arrayc0(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(82)) goto parse_data; - break; - } - - // optional bytes data = 10; - case 10: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_data: - DO_(::google::protobuf::internal::WireFormatLite::ReadBytes( - input, this->mutable_data())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void ObstacleMap::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // required int32 type = 2; - if (has_type()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(2, this->type(), output); - } - - // optional float resolution = 3; - if (has_resolution()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->resolution(), output); - } - - // optional int32 rows = 4; - if (has_rows()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(4, this->rows(), output); - } - - // optional int32 cols = 5; - if (has_cols()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(5, this->cols(), output); - } - - // optional int32 mapR0 = 6; - if (has_mapr0()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(6, this->mapr0(), output); - } - - // optional int32 mapC0 = 7; - if (has_mapc0()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(7, this->mapc0(), output); - } - - // optional int32 arrayR0 = 8; - if (has_arrayr0()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(8, this->arrayr0(), output); - } - - // optional int32 arrayC0 = 9; - if (has_arrayc0()) { - ::google::protobuf::internal::WireFormatLite::WriteInt32(9, this->arrayc0(), output); - } - - // optional bytes data = 10; - if (has_data()) { - ::google::protobuf::internal::WireFormatLite::WriteBytes( - 10, this->data(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* ObstacleMap::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // required int32 type = 2; - if (has_type()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(2, this->type(), target); - } - - // optional float resolution = 3; - if (has_resolution()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->resolution(), target); - } - - // optional int32 rows = 4; - if (has_rows()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(4, this->rows(), target); - } - - // optional int32 cols = 5; - if (has_cols()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(5, this->cols(), target); - } - - // optional int32 mapR0 = 6; - if (has_mapr0()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(6, this->mapr0(), target); - } - - // optional int32 mapC0 = 7; - if (has_mapc0()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(7, this->mapc0(), target); - } - - // optional int32 arrayR0 = 8; - if (has_arrayr0()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(8, this->arrayr0(), target); - } - - // optional int32 arrayC0 = 9; - if (has_arrayc0()) { - target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(9, this->arrayc0(), target); - } - - // optional bytes data = 10; - if (has_data()) { - target = - ::google::protobuf::internal::WireFormatLite::WriteBytesToArray( - 10, this->data(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int ObstacleMap::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - // required int32 type = 2; - if (has_type()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->type()); - } - - // optional float resolution = 3; - if (has_resolution()) { - total_size += 1 + 4; - } - - // optional int32 rows = 4; - if (has_rows()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->rows()); - } - - // optional int32 cols = 5; - if (has_cols()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->cols()); - } - - // optional int32 mapR0 = 6; - if (has_mapr0()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->mapr0()); - } - - // optional int32 mapC0 = 7; - if (has_mapc0()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->mapc0()); - } - - // optional int32 arrayR0 = 8; - if (has_arrayr0()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->arrayr0()); - } - - } - if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) { - // optional int32 arrayC0 = 9; - if (has_arrayc0()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::Int32Size( - this->arrayc0()); - } - - // optional bytes data = 10; - if (has_data()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::BytesSize( - this->data()); - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void ObstacleMap::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const ObstacleMap* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void ObstacleMap::MergeFrom(const ObstacleMap& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - if (from.has_type()) { - set_type(from.type()); - } - if (from.has_resolution()) { - set_resolution(from.resolution()); - } - if (from.has_rows()) { - set_rows(from.rows()); - } - if (from.has_cols()) { - set_cols(from.cols()); - } - if (from.has_mapr0()) { - set_mapr0(from.mapr0()); - } - if (from.has_mapc0()) { - set_mapc0(from.mapc0()); - } - if (from.has_arrayr0()) { - set_arrayr0(from.arrayr0()); - } - } - if (from._has_bits_[8 / 32] & (0xffu << (8 % 32))) { - if (from.has_arrayc0()) { - set_arrayc0(from.arrayc0()); - } - if (from.has_data()) { - set_data(from.data()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void ObstacleMap::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void ObstacleMap::CopyFrom(const ObstacleMap& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool ObstacleMap::IsInitialized() const { - if ((_has_bits_[0] & 0x00000003) != 0x00000003) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - return true; -} - -void ObstacleMap::Swap(ObstacleMap* other) { - if (other != this) { - std::swap(header_, other->header_); - std::swap(type_, other->type_); - std::swap(resolution_, other->resolution_); - std::swap(rows_, other->rows_); - std::swap(cols_, other->cols_); - std::swap(mapr0_, other->mapr0_); - std::swap(mapc0_, other->mapc0_); - std::swap(arrayr0_, other->arrayr0_); - std::swap(arrayc0_, other->arrayc0_); - std::swap(data_, other->data_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata ObstacleMap::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = ObstacleMap_descriptor_; - metadata.reflection = ObstacleMap_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int Path::kHeaderFieldNumber; -const int Path::kWaypointsFieldNumber; -#endif // !_MSC_VER - -Path::Path() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void Path::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -Path::Path(const Path& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void Path::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -Path::~Path() { - SharedDtor(); -} - -void Path::SharedDtor() { - if (this != default_instance_) { - delete header_; - } -} - -void Path::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* Path::descriptor() { - protobuf_AssignDescriptorsOnce(); - return Path_descriptor_; -} - -const Path& Path::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -Path* Path::default_instance_ = NULL; - -Path* Path::New() const { - return new Path; -} - -void Path::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - } - waypoints_.Clear(); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool Path::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_waypoints; - break; - } - - // repeated .px.Waypoint waypoints = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_waypoints: - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, add_waypoints())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_waypoints; - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void Path::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // repeated .px.Waypoint waypoints = 2; - for (int i = 0; i < this->waypoints_size(); i++) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 2, this->waypoints(i), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* Path::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // repeated .px.Waypoint waypoints = 2; - for (int i = 0; i < this->waypoints_size(); i++) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 2, this->waypoints(i), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int Path::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - } - // repeated .px.Waypoint waypoints = 2; - total_size += 1 * this->waypoints_size(); - for (int i = 0; i < this->waypoints_size(); i++) { - total_size += - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->waypoints(i)); - } - - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void Path::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const Path* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void Path::MergeFrom(const Path& from) { - GOOGLE_CHECK_NE(&from, this); - waypoints_.MergeFrom(from.waypoints_); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void Path::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void Path::CopyFrom(const Path& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool Path::IsInitialized() const { - if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - for (int i = 0; i < waypoints_size(); i++) { - if (!this->waypoints(i).IsInitialized()) return false; - } - return true; -} - -void Path::Swap(Path* other) { - if (other != this) { - std::swap(header_, other->header_); - waypoints_.Swap(&other->waypoints_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata Path::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = Path_descriptor_; - metadata.reflection = Path_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int PointCloudXYZI_PointXYZI::kXFieldNumber; -const int PointCloudXYZI_PointXYZI::kYFieldNumber; -const int PointCloudXYZI_PointXYZI::kZFieldNumber; -const int PointCloudXYZI_PointXYZI::kIntensityFieldNumber; -#endif // !_MSC_VER - -PointCloudXYZI_PointXYZI::PointCloudXYZI_PointXYZI() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void PointCloudXYZI_PointXYZI::InitAsDefaultInstance() { -} - -PointCloudXYZI_PointXYZI::PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void PointCloudXYZI_PointXYZI::SharedCtor() { - _cached_size_ = 0; - x_ = 0; - y_ = 0; - z_ = 0; - intensity_ = 0; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -PointCloudXYZI_PointXYZI::~PointCloudXYZI_PointXYZI() { - SharedDtor(); -} - -void PointCloudXYZI_PointXYZI::SharedDtor() { - if (this != default_instance_) { - } -} - -void PointCloudXYZI_PointXYZI::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* PointCloudXYZI_PointXYZI::descriptor() { - protobuf_AssignDescriptorsOnce(); - return PointCloudXYZI_PointXYZI_descriptor_; -} - -const PointCloudXYZI_PointXYZI& PointCloudXYZI_PointXYZI::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -PointCloudXYZI_PointXYZI* PointCloudXYZI_PointXYZI::default_instance_ = NULL; - -PointCloudXYZI_PointXYZI* PointCloudXYZI_PointXYZI::New() const { - return new PointCloudXYZI_PointXYZI; -} - -void PointCloudXYZI_PointXYZI::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - x_ = 0; - y_ = 0; - z_ = 0; - intensity_ = 0; - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool PointCloudXYZI_PointXYZI::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required float x = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &x_))); - set_has_x(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(21)) goto parse_y; - break; - } - - // required float y = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_y: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &y_))); - set_has_y(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(29)) goto parse_z; - break; - } - - // required float z = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_z: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &z_))); - set_has_z(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(37)) goto parse_intensity; - break; - } - - // required float intensity = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_intensity: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &intensity_))); - set_has_intensity(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void PointCloudXYZI_PointXYZI::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required float x = 1; - if (has_x()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output); - } - - // required float y = 2; - if (has_y()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output); - } - - // required float z = 3; - if (has_z()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output); - } - - // required float intensity = 4; - if (has_intensity()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->intensity(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* PointCloudXYZI_PointXYZI::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required float x = 1; - if (has_x()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target); - } - - // required float y = 2; - if (has_y()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target); - } - - // required float z = 3; - if (has_z()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target); - } - - // required float intensity = 4; - if (has_intensity()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->intensity(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int PointCloudXYZI_PointXYZI::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required float x = 1; - if (has_x()) { - total_size += 1 + 4; - } - - // required float y = 2; - if (has_y()) { - total_size += 1 + 4; - } - - // required float z = 3; - if (has_z()) { - total_size += 1 + 4; - } - - // required float intensity = 4; - if (has_intensity()) { - total_size += 1 + 4; - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void PointCloudXYZI_PointXYZI::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const PointCloudXYZI_PointXYZI* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void PointCloudXYZI_PointXYZI::MergeFrom(const PointCloudXYZI_PointXYZI& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_x()) { - set_x(from.x()); - } - if (from.has_y()) { - set_y(from.y()); - } - if (from.has_z()) { - set_z(from.z()); - } - if (from.has_intensity()) { - set_intensity(from.intensity()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void PointCloudXYZI_PointXYZI::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void PointCloudXYZI_PointXYZI::CopyFrom(const PointCloudXYZI_PointXYZI& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool PointCloudXYZI_PointXYZI::IsInitialized() const { - if ((_has_bits_[0] & 0x0000000f) != 0x0000000f) return false; - - return true; -} - -void PointCloudXYZI_PointXYZI::Swap(PointCloudXYZI_PointXYZI* other) { - if (other != this) { - std::swap(x_, other->x_); - std::swap(y_, other->y_); - std::swap(z_, other->z_); - std::swap(intensity_, other->intensity_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata PointCloudXYZI_PointXYZI::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = PointCloudXYZI_PointXYZI_descriptor_; - metadata.reflection = PointCloudXYZI_PointXYZI_reflection_; - return metadata; -} - - -// ------------------------------------------------------------------- - -#ifndef _MSC_VER -const int PointCloudXYZI::kHeaderFieldNumber; -const int PointCloudXYZI::kPointsFieldNumber; -#endif // !_MSC_VER - -PointCloudXYZI::PointCloudXYZI() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void PointCloudXYZI::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -PointCloudXYZI::PointCloudXYZI(const PointCloudXYZI& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void PointCloudXYZI::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -PointCloudXYZI::~PointCloudXYZI() { - SharedDtor(); -} - -void PointCloudXYZI::SharedDtor() { - if (this != default_instance_) { - delete header_; - } -} - -void PointCloudXYZI::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* PointCloudXYZI::descriptor() { - protobuf_AssignDescriptorsOnce(); - return PointCloudXYZI_descriptor_; -} - -const PointCloudXYZI& PointCloudXYZI::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -PointCloudXYZI* PointCloudXYZI::default_instance_ = NULL; - -PointCloudXYZI* PointCloudXYZI::New() const { - return new PointCloudXYZI; -} - -void PointCloudXYZI::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - } - points_.Clear(); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool PointCloudXYZI::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_points; - break; - } - - // repeated .px.PointCloudXYZI.PointXYZI points = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_points: - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, add_points())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_points; - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void PointCloudXYZI::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // repeated .px.PointCloudXYZI.PointXYZI points = 2; - for (int i = 0; i < this->points_size(); i++) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 2, this->points(i), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* PointCloudXYZI::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // repeated .px.PointCloudXYZI.PointXYZI points = 2; - for (int i = 0; i < this->points_size(); i++) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 2, this->points(i), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int PointCloudXYZI::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - } - // repeated .px.PointCloudXYZI.PointXYZI points = 2; - total_size += 1 * this->points_size(); - for (int i = 0; i < this->points_size(); i++) { - total_size += - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->points(i)); - } - - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void PointCloudXYZI::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const PointCloudXYZI* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void PointCloudXYZI::MergeFrom(const PointCloudXYZI& from) { - GOOGLE_CHECK_NE(&from, this); - points_.MergeFrom(from.points_); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void PointCloudXYZI::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void PointCloudXYZI::CopyFrom(const PointCloudXYZI& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool PointCloudXYZI::IsInitialized() const { - if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - for (int i = 0; i < points_size(); i++) { - if (!this->points(i).IsInitialized()) return false; - } - return true; -} - -void PointCloudXYZI::Swap(PointCloudXYZI* other) { - if (other != this) { - std::swap(header_, other->header_); - points_.Swap(&other->points_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata PointCloudXYZI::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = PointCloudXYZI_descriptor_; - metadata.reflection = PointCloudXYZI_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int PointCloudXYZRGB_PointXYZRGB::kXFieldNumber; -const int PointCloudXYZRGB_PointXYZRGB::kYFieldNumber; -const int PointCloudXYZRGB_PointXYZRGB::kZFieldNumber; -const int PointCloudXYZRGB_PointXYZRGB::kRgbFieldNumber; -#endif // !_MSC_VER - -PointCloudXYZRGB_PointXYZRGB::PointCloudXYZRGB_PointXYZRGB() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void PointCloudXYZRGB_PointXYZRGB::InitAsDefaultInstance() { -} - -PointCloudXYZRGB_PointXYZRGB::PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void PointCloudXYZRGB_PointXYZRGB::SharedCtor() { - _cached_size_ = 0; - x_ = 0; - y_ = 0; - z_ = 0; - rgb_ = 0; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -PointCloudXYZRGB_PointXYZRGB::~PointCloudXYZRGB_PointXYZRGB() { - SharedDtor(); -} - -void PointCloudXYZRGB_PointXYZRGB::SharedDtor() { - if (this != default_instance_) { - } -} - -void PointCloudXYZRGB_PointXYZRGB::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* PointCloudXYZRGB_PointXYZRGB::descriptor() { - protobuf_AssignDescriptorsOnce(); - return PointCloudXYZRGB_PointXYZRGB_descriptor_; -} - -const PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB_PointXYZRGB::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB_PointXYZRGB::default_instance_ = NULL; - -PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB_PointXYZRGB::New() const { - return new PointCloudXYZRGB_PointXYZRGB; -} - -void PointCloudXYZRGB_PointXYZRGB::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - x_ = 0; - y_ = 0; - z_ = 0; - rgb_ = 0; - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool PointCloudXYZRGB_PointXYZRGB::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required float x = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &x_))); - set_has_x(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(21)) goto parse_y; - break; - } - - // required float y = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_y: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &y_))); - set_has_y(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(29)) goto parse_z; - break; - } - - // required float z = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_z: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &z_))); - set_has_z(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(37)) goto parse_rgb; - break; - } - - // required float rgb = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_rgb: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &rgb_))); - set_has_rgb(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required float x = 1; - if (has_x()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output); - } - - // required float y = 2; - if (has_y()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output); - } - - // required float z = 3; - if (has_z()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output); - } - - // required float rgb = 4; - if (has_rgb()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->rgb(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required float x = 1; - if (has_x()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target); - } - - // required float y = 2; - if (has_y()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target); - } - - // required float z = 3; - if (has_z()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target); - } - - // required float rgb = 4; - if (has_rgb()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->rgb(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int PointCloudXYZRGB_PointXYZRGB::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required float x = 1; - if (has_x()) { - total_size += 1 + 4; - } - - // required float y = 2; - if (has_y()) { - total_size += 1 + 4; - } - - // required float z = 3; - if (has_z()) { - total_size += 1 + 4; - } - - // required float rgb = 4; - if (has_rgb()) { - total_size += 1 + 4; - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const PointCloudXYZRGB_PointXYZRGB* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const PointCloudXYZRGB_PointXYZRGB& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_x()) { - set_x(from.x()); - } - if (from.has_y()) { - set_y(from.y()); - } - if (from.has_z()) { - set_z(from.z()); - } - if (from.has_rgb()) { - set_rgb(from.rgb()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void PointCloudXYZRGB_PointXYZRGB::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void PointCloudXYZRGB_PointXYZRGB::CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool PointCloudXYZRGB_PointXYZRGB::IsInitialized() const { - if ((_has_bits_[0] & 0x0000000f) != 0x0000000f) return false; - - return true; -} - -void PointCloudXYZRGB_PointXYZRGB::Swap(PointCloudXYZRGB_PointXYZRGB* other) { - if (other != this) { - std::swap(x_, other->x_); - std::swap(y_, other->y_); - std::swap(z_, other->z_); - std::swap(rgb_, other->rgb_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata PointCloudXYZRGB_PointXYZRGB::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = PointCloudXYZRGB_PointXYZRGB_descriptor_; - metadata.reflection = PointCloudXYZRGB_PointXYZRGB_reflection_; - return metadata; -} - - -// ------------------------------------------------------------------- - -#ifndef _MSC_VER -const int PointCloudXYZRGB::kHeaderFieldNumber; -const int PointCloudXYZRGB::kPointsFieldNumber; -#endif // !_MSC_VER - -PointCloudXYZRGB::PointCloudXYZRGB() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void PointCloudXYZRGB::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -PointCloudXYZRGB::PointCloudXYZRGB(const PointCloudXYZRGB& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void PointCloudXYZRGB::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -PointCloudXYZRGB::~PointCloudXYZRGB() { - SharedDtor(); -} - -void PointCloudXYZRGB::SharedDtor() { - if (this != default_instance_) { - delete header_; - } -} - -void PointCloudXYZRGB::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* PointCloudXYZRGB::descriptor() { - protobuf_AssignDescriptorsOnce(); - return PointCloudXYZRGB_descriptor_; -} - -const PointCloudXYZRGB& PointCloudXYZRGB::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -PointCloudXYZRGB* PointCloudXYZRGB::default_instance_ = NULL; - -PointCloudXYZRGB* PointCloudXYZRGB::New() const { - return new PointCloudXYZRGB; -} - -void PointCloudXYZRGB::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - } - points_.Clear(); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool PointCloudXYZRGB::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_points; - break; - } - - // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_points: - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, add_points())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(18)) goto parse_points; - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void PointCloudXYZRGB::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; - for (int i = 0; i < this->points_size(); i++) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 2, this->points(i), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* PointCloudXYZRGB::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; - for (int i = 0; i < this->points_size(); i++) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 2, this->points(i), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int PointCloudXYZRGB::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - } - // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2; - total_size += 1 * this->points_size(); - for (int i = 0; i < this->points_size(); i++) { - total_size += - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->points(i)); - } - - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void PointCloudXYZRGB::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const PointCloudXYZRGB* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void PointCloudXYZRGB::MergeFrom(const PointCloudXYZRGB& from) { - GOOGLE_CHECK_NE(&from, this); - points_.MergeFrom(from.points_); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void PointCloudXYZRGB::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void PointCloudXYZRGB::CopyFrom(const PointCloudXYZRGB& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool PointCloudXYZRGB::IsInitialized() const { - if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - for (int i = 0; i < points_size(); i++) { - if (!this->points(i).IsInitialized()) return false; - } - return true; -} - -void PointCloudXYZRGB::Swap(PointCloudXYZRGB* other) { - if (other != this) { - std::swap(header_, other->header_); - points_.Swap(&other->points_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata PointCloudXYZRGB::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = PointCloudXYZRGB_descriptor_; - metadata.reflection = PointCloudXYZRGB_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int RGBDImage::kHeaderFieldNumber; -const int RGBDImage::kColsFieldNumber; -const int RGBDImage::kRowsFieldNumber; -const int RGBDImage::kStep1FieldNumber; -const int RGBDImage::kType1FieldNumber; -const int RGBDImage::kImageData1FieldNumber; -const int RGBDImage::kStep2FieldNumber; -const int RGBDImage::kType2FieldNumber; -const int RGBDImage::kImageData2FieldNumber; -const int RGBDImage::kCameraConfigFieldNumber; -const int RGBDImage::kCameraTypeFieldNumber; -const int RGBDImage::kRollFieldNumber; -const int RGBDImage::kPitchFieldNumber; -const int RGBDImage::kYawFieldNumber; -const int RGBDImage::kLonFieldNumber; -const int RGBDImage::kLatFieldNumber; -const int RGBDImage::kAltFieldNumber; -const int RGBDImage::kGroundXFieldNumber; -const int RGBDImage::kGroundYFieldNumber; -const int RGBDImage::kGroundZFieldNumber; -const int RGBDImage::kCameraMatrixFieldNumber; -#endif // !_MSC_VER - -RGBDImage::RGBDImage() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void RGBDImage::InitAsDefaultInstance() { - header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance()); -} - -RGBDImage::RGBDImage(const RGBDImage& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void RGBDImage::SharedCtor() { - _cached_size_ = 0; - header_ = NULL; - cols_ = 0u; - rows_ = 0u; - step1_ = 0u; - type1_ = 0u; - imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - step2_ = 0u; - type2_ = 0u; - imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString); - camera_config_ = 0u; - camera_type_ = 0u; - roll_ = 0; - pitch_ = 0; - yaw_ = 0; - lon_ = 0; - lat_ = 0; - alt_ = 0; - ground_x_ = 0; - ground_y_ = 0; - ground_z_ = 0; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -RGBDImage::~RGBDImage() { - SharedDtor(); -} - -void RGBDImage::SharedDtor() { - if (imagedata1_ != &::google::protobuf::internal::kEmptyString) { - delete imagedata1_; - } - if (imagedata2_ != &::google::protobuf::internal::kEmptyString) { - delete imagedata2_; - } - if (this != default_instance_) { - delete header_; - } -} - -void RGBDImage::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* RGBDImage::descriptor() { - protobuf_AssignDescriptorsOnce(); - return RGBDImage_descriptor_; -} - -const RGBDImage& RGBDImage::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -RGBDImage* RGBDImage::default_instance_ = NULL; - -RGBDImage* RGBDImage::New() const { - return new RGBDImage; -} - -void RGBDImage::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (has_header()) { - if (header_ != NULL) header_->::px::HeaderInfo::Clear(); - } - cols_ = 0u; - rows_ = 0u; - step1_ = 0u; - type1_ = 0u; - if (has_imagedata1()) { - if (imagedata1_ != &::google::protobuf::internal::kEmptyString) { - imagedata1_->clear(); - } - } - step2_ = 0u; - type2_ = 0u; - } - if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) { - if (has_imagedata2()) { - if (imagedata2_ != &::google::protobuf::internal::kEmptyString) { - imagedata2_->clear(); - } - } - camera_config_ = 0u; - camera_type_ = 0u; - roll_ = 0; - pitch_ = 0; - yaw_ = 0; - lon_ = 0; - lat_ = 0; - } - if (_has_bits_[16 / 32] & (0xffu << (16 % 32))) { - alt_ = 0; - ground_x_ = 0; - ground_y_ = 0; - ground_z_ = 0; - } - camera_matrix_.Clear(); - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool RGBDImage::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required .px.HeaderInfo header = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual( - input, mutable_header())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(16)) goto parse_cols; - break; - } - - // required uint32 cols = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_cols: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &cols_))); - set_has_cols(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(24)) goto parse_rows; - break; - } - - // required uint32 rows = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_rows: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &rows_))); - set_has_rows(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(32)) goto parse_step1; - break; - } - - // required uint32 step1 = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_step1: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &step1_))); - set_has_step1(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(40)) goto parse_type1; - break; - } - - // required uint32 type1 = 5; - case 5: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_type1: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &type1_))); - set_has_type1(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(50)) goto parse_imageData1; - break; - } - - // required bytes imageData1 = 6; - case 6: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_imageData1: - DO_(::google::protobuf::internal::WireFormatLite::ReadBytes( - input, this->mutable_imagedata1())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(56)) goto parse_step2; - break; - } - - // required uint32 step2 = 7; - case 7: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_step2: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &step2_))); - set_has_step2(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(64)) goto parse_type2; - break; - } - - // required uint32 type2 = 8; - case 8: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_type2: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &type2_))); - set_has_type2(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(74)) goto parse_imageData2; - break; - } - - // required bytes imageData2 = 9; - case 9: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - parse_imageData2: - DO_(::google::protobuf::internal::WireFormatLite::ReadBytes( - input, this->mutable_imagedata2())); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(80)) goto parse_camera_config; - break; - } - - // optional uint32 camera_config = 10; - case 10: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_camera_config: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &camera_config_))); - set_has_camera_config(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(88)) goto parse_camera_type; - break; - } - - // optional uint32 camera_type = 11; - case 11: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) { - parse_camera_type: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>( - input, &camera_type_))); - set_has_camera_type(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(101)) goto parse_roll; - break; - } - - // optional float roll = 12; - case 12: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_roll: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &roll_))); - set_has_roll(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(109)) goto parse_pitch; - break; - } - - // optional float pitch = 13; - case 13: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_pitch: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &pitch_))); - set_has_pitch(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(117)) goto parse_yaw; - break; - } - - // optional float yaw = 14; - case 14: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_yaw: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &yaw_))); - set_has_yaw(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(125)) goto parse_lon; - break; - } - - // optional float lon = 15; - case 15: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_lon: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &lon_))); - set_has_lon(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(133)) goto parse_lat; - break; - } - - // optional float lat = 16; - case 16: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_lat: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &lat_))); - set_has_lat(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(141)) goto parse_alt; - break; - } - - // optional float alt = 17; - case 17: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_alt: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &alt_))); - set_has_alt(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(149)) goto parse_ground_x; - break; - } - - // optional float ground_x = 18; - case 18: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_ground_x: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &ground_x_))); - set_has_ground_x(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(157)) goto parse_ground_y; - break; - } - - // optional float ground_y = 19; - case 19: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_ground_y: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &ground_y_))); - set_has_ground_y(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(165)) goto parse_ground_z; - break; - } - - // optional float ground_z = 20; - case 20: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_ground_z: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, &ground_z_))); - set_has_ground_z(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(173)) goto parse_camera_matrix; - break; - } - - // repeated float camera_matrix = 21; - case 21: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) { - parse_camera_matrix: - DO_((::google::protobuf::internal::WireFormatLite::ReadRepeatedPrimitive< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - 2, 173, input, this->mutable_camera_matrix()))); - } else if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) - == ::google::protobuf::internal::WireFormatLite:: - WIRETYPE_LENGTH_DELIMITED) { - DO_((::google::protobuf::internal::WireFormatLite::ReadPackedPrimitiveNoInline< - float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>( - input, this->mutable_camera_matrix()))); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(173)) goto parse_camera_matrix; - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void RGBDImage::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray( - 1, this->header(), output); - } - - // required uint32 cols = 2; - if (has_cols()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(2, this->cols(), output); - } - - // required uint32 rows = 3; - if (has_rows()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(3, this->rows(), output); - } - - // required uint32 step1 = 4; - if (has_step1()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(4, this->step1(), output); - } - - // required uint32 type1 = 5; - if (has_type1()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(5, this->type1(), output); - } - - // required bytes imageData1 = 6; - if (has_imagedata1()) { - ::google::protobuf::internal::WireFormatLite::WriteBytes( - 6, this->imagedata1(), output); - } - - // required uint32 step2 = 7; - if (has_step2()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(7, this->step2(), output); - } - - // required uint32 type2 = 8; - if (has_type2()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(8, this->type2(), output); - } - - // required bytes imageData2 = 9; - if (has_imagedata2()) { - ::google::protobuf::internal::WireFormatLite::WriteBytes( - 9, this->imagedata2(), output); - } - - // optional uint32 camera_config = 10; - if (has_camera_config()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(10, this->camera_config(), output); - } - - // optional uint32 camera_type = 11; - if (has_camera_type()) { - ::google::protobuf::internal::WireFormatLite::WriteUInt32(11, this->camera_type(), output); - } - - // optional float roll = 12; - if (has_roll()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(12, this->roll(), output); - } - - // optional float pitch = 13; - if (has_pitch()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(13, this->pitch(), output); - } - - // optional float yaw = 14; - if (has_yaw()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(14, this->yaw(), output); - } - - // optional float lon = 15; - if (has_lon()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(15, this->lon(), output); - } - - // optional float lat = 16; - if (has_lat()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(16, this->lat(), output); - } - - // optional float alt = 17; - if (has_alt()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(17, this->alt(), output); - } - - // optional float ground_x = 18; - if (has_ground_x()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(18, this->ground_x(), output); - } - - // optional float ground_y = 19; - if (has_ground_y()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(19, this->ground_y(), output); - } - - // optional float ground_z = 20; - if (has_ground_z()) { - ::google::protobuf::internal::WireFormatLite::WriteFloat(20, this->ground_z(), output); - } - - // repeated float camera_matrix = 21; - for (int i = 0; i < this->camera_matrix_size(); i++) { - ::google::protobuf::internal::WireFormatLite::WriteFloat( - 21, this->camera_matrix(i), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* RGBDImage::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required .px.HeaderInfo header = 1; - if (has_header()) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteMessageNoVirtualToArray( - 1, this->header(), target); - } - - // required uint32 cols = 2; - if (has_cols()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(2, this->cols(), target); - } - - // required uint32 rows = 3; - if (has_rows()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(3, this->rows(), target); - } - - // required uint32 step1 = 4; - if (has_step1()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(4, this->step1(), target); - } - - // required uint32 type1 = 5; - if (has_type1()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(5, this->type1(), target); - } - - // required bytes imageData1 = 6; - if (has_imagedata1()) { - target = - ::google::protobuf::internal::WireFormatLite::WriteBytesToArray( - 6, this->imagedata1(), target); - } - - // required uint32 step2 = 7; - if (has_step2()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(7, this->step2(), target); - } - - // required uint32 type2 = 8; - if (has_type2()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(8, this->type2(), target); - } - - // required bytes imageData2 = 9; - if (has_imagedata2()) { - target = - ::google::protobuf::internal::WireFormatLite::WriteBytesToArray( - 9, this->imagedata2(), target); - } - - // optional uint32 camera_config = 10; - if (has_camera_config()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(10, this->camera_config(), target); - } - - // optional uint32 camera_type = 11; - if (has_camera_type()) { - target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(11, this->camera_type(), target); - } - - // optional float roll = 12; - if (has_roll()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(12, this->roll(), target); - } - - // optional float pitch = 13; - if (has_pitch()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(13, this->pitch(), target); - } - - // optional float yaw = 14; - if (has_yaw()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(14, this->yaw(), target); - } - - // optional float lon = 15; - if (has_lon()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(15, this->lon(), target); - } - - // optional float lat = 16; - if (has_lat()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(16, this->lat(), target); - } - - // optional float alt = 17; - if (has_alt()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(17, this->alt(), target); - } - - // optional float ground_x = 18; - if (has_ground_x()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(18, this->ground_x(), target); - } - - // optional float ground_y = 19; - if (has_ground_y()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(19, this->ground_y(), target); - } - - // optional float ground_z = 20; - if (has_ground_z()) { - target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(20, this->ground_z(), target); - } - - // repeated float camera_matrix = 21; - for (int i = 0; i < this->camera_matrix_size(); i++) { - target = ::google::protobuf::internal::WireFormatLite:: - WriteFloatToArray(21, this->camera_matrix(i), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int RGBDImage::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required .px.HeaderInfo header = 1; - if (has_header()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual( - this->header()); - } - - // required uint32 cols = 2; - if (has_cols()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->cols()); - } - - // required uint32 rows = 3; - if (has_rows()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->rows()); - } - - // required uint32 step1 = 4; - if (has_step1()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->step1()); - } - - // required uint32 type1 = 5; - if (has_type1()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->type1()); - } - - // required bytes imageData1 = 6; - if (has_imagedata1()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::BytesSize( - this->imagedata1()); - } - - // required uint32 step2 = 7; - if (has_step2()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->step2()); - } - - // required uint32 type2 = 8; - if (has_type2()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->type2()); - } - - } - if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) { - // required bytes imageData2 = 9; - if (has_imagedata2()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::BytesSize( - this->imagedata2()); - } - - // optional uint32 camera_config = 10; - if (has_camera_config()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->camera_config()); - } - - // optional uint32 camera_type = 11; - if (has_camera_type()) { - total_size += 1 + - ::google::protobuf::internal::WireFormatLite::UInt32Size( - this->camera_type()); - } - - // optional float roll = 12; - if (has_roll()) { - total_size += 1 + 4; - } - - // optional float pitch = 13; - if (has_pitch()) { - total_size += 1 + 4; - } - - // optional float yaw = 14; - if (has_yaw()) { - total_size += 1 + 4; - } - - // optional float lon = 15; - if (has_lon()) { - total_size += 1 + 4; - } - - // optional float lat = 16; - if (has_lat()) { - total_size += 2 + 4; - } - - } - if (_has_bits_[16 / 32] & (0xffu << (16 % 32))) { - // optional float alt = 17; - if (has_alt()) { - total_size += 2 + 4; - } - - // optional float ground_x = 18; - if (has_ground_x()) { - total_size += 2 + 4; - } - - // optional float ground_y = 19; - if (has_ground_y()) { - total_size += 2 + 4; - } - - // optional float ground_z = 20; - if (has_ground_z()) { - total_size += 2 + 4; - } - - } - // repeated float camera_matrix = 21; - { - int data_size = 0; - data_size = 4 * this->camera_matrix_size(); - total_size += 2 * this->camera_matrix_size() + data_size; - } - - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void RGBDImage::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const RGBDImage* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void RGBDImage::MergeFrom(const RGBDImage& from) { - GOOGLE_CHECK_NE(&from, this); - camera_matrix_.MergeFrom(from.camera_matrix_); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_header()) { - mutable_header()->::px::HeaderInfo::MergeFrom(from.header()); - } - if (from.has_cols()) { - set_cols(from.cols()); - } - if (from.has_rows()) { - set_rows(from.rows()); - } - if (from.has_step1()) { - set_step1(from.step1()); - } - if (from.has_type1()) { - set_type1(from.type1()); - } - if (from.has_imagedata1()) { - set_imagedata1(from.imagedata1()); - } - if (from.has_step2()) { - set_step2(from.step2()); - } - if (from.has_type2()) { - set_type2(from.type2()); - } - } - if (from._has_bits_[8 / 32] & (0xffu << (8 % 32))) { - if (from.has_imagedata2()) { - set_imagedata2(from.imagedata2()); - } - if (from.has_camera_config()) { - set_camera_config(from.camera_config()); - } - if (from.has_camera_type()) { - set_camera_type(from.camera_type()); - } - if (from.has_roll()) { - set_roll(from.roll()); - } - if (from.has_pitch()) { - set_pitch(from.pitch()); - } - if (from.has_yaw()) { - set_yaw(from.yaw()); - } - if (from.has_lon()) { - set_lon(from.lon()); - } - if (from.has_lat()) { - set_lat(from.lat()); - } - } - if (from._has_bits_[16 / 32] & (0xffu << (16 % 32))) { - if (from.has_alt()) { - set_alt(from.alt()); - } - if (from.has_ground_x()) { - set_ground_x(from.ground_x()); - } - if (from.has_ground_y()) { - set_ground_y(from.ground_y()); - } - if (from.has_ground_z()) { - set_ground_z(from.ground_z()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void RGBDImage::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void RGBDImage::CopyFrom(const RGBDImage& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool RGBDImage::IsInitialized() const { - if ((_has_bits_[0] & 0x000001ff) != 0x000001ff) return false; - - if (has_header()) { - if (!this->header().IsInitialized()) return false; - } - return true; -} - -void RGBDImage::Swap(RGBDImage* other) { - if (other != this) { - std::swap(header_, other->header_); - std::swap(cols_, other->cols_); - std::swap(rows_, other->rows_); - std::swap(step1_, other->step1_); - std::swap(type1_, other->type1_); - std::swap(imagedata1_, other->imagedata1_); - std::swap(step2_, other->step2_); - std::swap(type2_, other->type2_); - std::swap(imagedata2_, other->imagedata2_); - std::swap(camera_config_, other->camera_config_); - std::swap(camera_type_, other->camera_type_); - std::swap(roll_, other->roll_); - std::swap(pitch_, other->pitch_); - std::swap(yaw_, other->yaw_); - std::swap(lon_, other->lon_); - std::swap(lat_, other->lat_); - std::swap(alt_, other->alt_); - std::swap(ground_x_, other->ground_x_); - std::swap(ground_y_, other->ground_y_); - std::swap(ground_z_, other->ground_z_); - camera_matrix_.Swap(&other->camera_matrix_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata RGBDImage::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = RGBDImage_descriptor_; - metadata.reflection = RGBDImage_reflection_; - return metadata; -} - - -// =================================================================== - -#ifndef _MSC_VER -const int Waypoint::kXFieldNumber; -const int Waypoint::kYFieldNumber; -const int Waypoint::kZFieldNumber; -const int Waypoint::kRollFieldNumber; -const int Waypoint::kPitchFieldNumber; -const int Waypoint::kYawFieldNumber; -#endif // !_MSC_VER - -Waypoint::Waypoint() - : ::google::protobuf::Message() { - SharedCtor(); -} - -void Waypoint::InitAsDefaultInstance() { -} - -Waypoint::Waypoint(const Waypoint& from) - : ::google::protobuf::Message() { - SharedCtor(); - MergeFrom(from); -} - -void Waypoint::SharedCtor() { - _cached_size_ = 0; - x_ = 0; - y_ = 0; - z_ = 0; - roll_ = 0; - pitch_ = 0; - yaw_ = 0; - ::memset(_has_bits_, 0, sizeof(_has_bits_)); -} - -Waypoint::~Waypoint() { - SharedDtor(); -} - -void Waypoint::SharedDtor() { - if (this != default_instance_) { - } -} - -void Waypoint::SetCachedSize(int size) const { - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); -} -const ::google::protobuf::Descriptor* Waypoint::descriptor() { - protobuf_AssignDescriptorsOnce(); - return Waypoint_descriptor_; -} - -const Waypoint& Waypoint::default_instance() { - if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto(); return *default_instance_; -} - -Waypoint* Waypoint::default_instance_ = NULL; - -Waypoint* Waypoint::New() const { - return new Waypoint; -} - -void Waypoint::Clear() { - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - x_ = 0; - y_ = 0; - z_ = 0; - roll_ = 0; - pitch_ = 0; - yaw_ = 0; - } - ::memset(_has_bits_, 0, sizeof(_has_bits_)); - mutable_unknown_fields()->Clear(); -} - -bool Waypoint::MergePartialFromCodedStream( - ::google::protobuf::io::CodedInputStream* input) { -#define DO_(EXPRESSION) if (!(EXPRESSION)) return false - ::google::protobuf::uint32 tag; - while ((tag = input->ReadTag()) != 0) { - switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) { - // required double x = 1; - case 1: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &x_))); - set_has_x(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(17)) goto parse_y; - break; - } - - // required double y = 2; - case 2: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_y: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &y_))); - set_has_y(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(25)) goto parse_z; - break; - } - - // optional double z = 3; - case 3: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_z: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &z_))); - set_has_z(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(33)) goto parse_roll; - break; - } - - // optional double roll = 4; - case 4: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_roll: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &roll_))); - set_has_roll(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(41)) goto parse_pitch; - break; - } - - // optional double pitch = 5; - case 5: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_pitch: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &pitch_))); - set_has_pitch(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectTag(49)) goto parse_yaw; - break; - } - - // optional double yaw = 6; - case 6: { - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) { - parse_yaw: - DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive< - double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>( - input, &yaw_))); - set_has_yaw(); - } else { - goto handle_uninterpreted; - } - if (input->ExpectAtEnd()) return true; - break; - } - - default: { - handle_uninterpreted: - if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) == - ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) { - return true; - } - DO_(::google::protobuf::internal::WireFormat::SkipField( - input, tag, mutable_unknown_fields())); - break; - } - } - } - return true; -#undef DO_ -} - -void Waypoint::SerializeWithCachedSizes( - ::google::protobuf::io::CodedOutputStream* output) const { - // required double x = 1; - if (has_x()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(1, this->x(), output); - } - - // required double y = 2; - if (has_y()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(2, this->y(), output); - } - - // optional double z = 3; - if (has_z()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(3, this->z(), output); - } - - // optional double roll = 4; - if (has_roll()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(4, this->roll(), output); - } - - // optional double pitch = 5; - if (has_pitch()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(5, this->pitch(), output); - } - - // optional double yaw = 6; - if (has_yaw()) { - ::google::protobuf::internal::WireFormatLite::WriteDouble(6, this->yaw(), output); - } - - if (!unknown_fields().empty()) { - ::google::protobuf::internal::WireFormat::SerializeUnknownFields( - unknown_fields(), output); - } -} - -::google::protobuf::uint8* Waypoint::SerializeWithCachedSizesToArray( - ::google::protobuf::uint8* target) const { - // required double x = 1; - if (has_x()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(1, this->x(), target); - } - - // required double y = 2; - if (has_y()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(2, this->y(), target); - } - - // optional double z = 3; - if (has_z()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(3, this->z(), target); - } - - // optional double roll = 4; - if (has_roll()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(4, this->roll(), target); - } - - // optional double pitch = 5; - if (has_pitch()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(5, this->pitch(), target); - } - - // optional double yaw = 6; - if (has_yaw()) { - target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(6, this->yaw(), target); - } - - if (!unknown_fields().empty()) { - target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray( - unknown_fields(), target); - } - return target; -} - -int Waypoint::ByteSize() const { - int total_size = 0; - - if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) { - // required double x = 1; - if (has_x()) { - total_size += 1 + 8; - } - - // required double y = 2; - if (has_y()) { - total_size += 1 + 8; - } - - // optional double z = 3; - if (has_z()) { - total_size += 1 + 8; - } - - // optional double roll = 4; - if (has_roll()) { - total_size += 1 + 8; - } - - // optional double pitch = 5; - if (has_pitch()) { - total_size += 1 + 8; - } - - // optional double yaw = 6; - if (has_yaw()) { - total_size += 1 + 8; - } - - } - if (!unknown_fields().empty()) { - total_size += - ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize( - unknown_fields()); - } - GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN(); - _cached_size_ = total_size; - GOOGLE_SAFE_CONCURRENT_WRITES_END(); - return total_size; -} - -void Waypoint::MergeFrom(const ::google::protobuf::Message& from) { - GOOGLE_CHECK_NE(&from, this); - const Waypoint* source = - ::google::protobuf::internal::dynamic_cast_if_available( - &from); - if (source == NULL) { - ::google::protobuf::internal::ReflectionOps::Merge(from, this); - } else { - MergeFrom(*source); - } -} - -void Waypoint::MergeFrom(const Waypoint& from) { - GOOGLE_CHECK_NE(&from, this); - if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { - if (from.has_x()) { - set_x(from.x()); - } - if (from.has_y()) { - set_y(from.y()); - } - if (from.has_z()) { - set_z(from.z()); - } - if (from.has_roll()) { - set_roll(from.roll()); - } - if (from.has_pitch()) { - set_pitch(from.pitch()); - } - if (from.has_yaw()) { - set_yaw(from.yaw()); - } - } - mutable_unknown_fields()->MergeFrom(from.unknown_fields()); -} - -void Waypoint::CopyFrom(const ::google::protobuf::Message& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -void Waypoint::CopyFrom(const Waypoint& from) { - if (&from == this) return; - Clear(); - MergeFrom(from); -} - -bool Waypoint::IsInitialized() const { - if ((_has_bits_[0] & 0x00000003) != 0x00000003) return false; - - return true; -} - -void Waypoint::Swap(Waypoint* other) { - if (other != this) { - std::swap(x_, other->x_); - std::swap(y_, other->y_); - std::swap(z_, other->z_); - std::swap(roll_, other->roll_); - std::swap(pitch_, other->pitch_); - std::swap(yaw_, other->yaw_); - std::swap(_has_bits_[0], other->_has_bits_[0]); - _unknown_fields_.Swap(&other->_unknown_fields_); - std::swap(_cached_size_, other->_cached_size_); - } -} - -::google::protobuf::Metadata Waypoint::GetMetadata() const { - protobuf_AssignDescriptorsOnce(); - ::google::protobuf::Metadata metadata; - metadata.descriptor = Waypoint_descriptor_; - metadata.reflection = Waypoint_reflection_; - return metadata; -} - - -// @@protoc_insertion_point(namespace_scope) - -} // namespace px - -// @@protoc_insertion_point(global_scope) diff --git a/mavlink/share/pyshared/pymavlink/generator/C/test/posix/.gitignore b/mavlink/share/pyshared/pymavlink/generator/C/test/posix/.gitignore deleted file mode 100644 index 7c98650cc..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/test/posix/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -/testmav0.9 -/testmav1.0 -/testmav1.0_nonstrict diff --git a/mavlink/share/pyshared/pymavlink/generator/C/test/posix/testmav.c b/mavlink/share/pyshared/pymavlink/generator/C/test/posix/testmav.c deleted file mode 100644 index 2fd7fa378..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/test/posix/testmav.c +++ /dev/null @@ -1,159 +0,0 @@ -/* - simple MAVLink testsuite for C - */ -#include -#include -#include -#include -#include - -#define MAVLINK_USE_CONVENIENCE_FUNCTIONS -#define MAVLINK_COMM_NUM_BUFFERS 2 - -// this trick allows us to make mavlink_message_t as small as possible -// for this dialect, which saves some memory -#include -#define MAVLINK_MAX_PAYLOAD_LEN MAVLINK_MAX_DIALECT_PAYLOAD_SIZE - -#include -static mavlink_system_t mavlink_system = {42,11,}; - -#define MAVLINK_ASSERT(x) assert(x) -static void comm_send_ch(mavlink_channel_t chan, uint8_t c); - -static mavlink_message_t last_msg; - -#include -#include - -static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS]; - -static const unsigned message_lengths[] = MAVLINK_MESSAGE_LENGTHS; -static unsigned error_count; - -static const mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO; - -static void print_one_field(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx) -{ -#define PRINT_FORMAT(f, def) (f->print_format?f->print_format:def) - switch (f->type) { - case MAVLINK_TYPE_CHAR: - printf(PRINT_FORMAT(f, "%c"), _MAV_RETURN_char(msg, f->wire_offset+idx*1)); - break; - case MAVLINK_TYPE_UINT8_T: - printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint8_t(msg, f->wire_offset+idx*1)); - break; - case MAVLINK_TYPE_INT8_T: - printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int8_t(msg, f->wire_offset+idx*1)); - break; - case MAVLINK_TYPE_UINT16_T: - printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint16_t(msg, f->wire_offset+idx*2)); - break; - case MAVLINK_TYPE_INT16_T: - printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int16_t(msg, f->wire_offset+idx*2)); - break; - case MAVLINK_TYPE_UINT32_T: - printf(PRINT_FORMAT(f, "%lu"), (unsigned long)_MAV_RETURN_uint32_t(msg, f->wire_offset+idx*4)); - break; - case MAVLINK_TYPE_INT32_T: - printf(PRINT_FORMAT(f, "%ld"), (long)_MAV_RETURN_int32_t(msg, f->wire_offset+idx*4)); - break; - case MAVLINK_TYPE_UINT64_T: - printf(PRINT_FORMAT(f, "%llu"), (unsigned long long)_MAV_RETURN_uint64_t(msg, f->wire_offset+idx*8)); - break; - case MAVLINK_TYPE_INT64_T: - printf(PRINT_FORMAT(f, "%lld"), (long long)_MAV_RETURN_int64_t(msg, f->wire_offset+idx*8)); - break; - case MAVLINK_TYPE_FLOAT: - printf(PRINT_FORMAT(f, "%f"), (double)_MAV_RETURN_float(msg, f->wire_offset+idx*4)); - break; - case MAVLINK_TYPE_DOUBLE: - printf(PRINT_FORMAT(f, "%f"), _MAV_RETURN_double(msg, f->wire_offset+idx*8)); - break; - } -} - -static void print_field(mavlink_message_t *msg, const mavlink_field_info_t *f) -{ - printf("%s: ", f->name); - if (f->array_length == 0) { - print_one_field(msg, f, 0); - printf(" "); - } else { - unsigned i; - /* print an array */ - if (f->type == MAVLINK_TYPE_CHAR) { - printf("'%.*s'", f->array_length, - f->wire_offset+(const char *)_MAV_PAYLOAD(msg)); - - } else { - printf("[ "); - for (i=0; iarray_length; i++) { - print_one_field(msg, f, i); - if (i < f->array_length) { - printf(", "); - } - } - printf("]"); - } - } - printf(" "); -} - -static void print_message(mavlink_message_t *msg) -{ - const mavlink_message_info_t *m = &message_info[msg->msgid]; - const mavlink_field_info_t *f = m->fields; - unsigned i; - printf("%s { ", m->name); - for (i=0; inum_fields; i++) { - print_field(msg, &f[i]); - } - printf("}\n"); -} - -static void comm_send_ch(mavlink_channel_t chan, uint8_t c) -{ - mavlink_status_t status; - if (mavlink_parse_char(chan, c, &last_msg, &status)) { - print_message(&last_msg); - chan_counts[chan]++; - /* channel 0 gets 3 messages per message, because of - the channel defaults for _pack() and _encode() */ - if (chan == MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)(chan_counts[chan]*3)) { - printf("Channel 0 sequence mismatch error at packet %u (rx_seq=%u)\n", - chan_counts[chan], status.current_rx_seq); - error_count++; - } else if (chan > MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)chan_counts[chan]) { - printf("Channel %u sequence mismatch error at packet %u (rx_seq=%u)\n", - (unsigned)chan, chan_counts[chan], status.current_rx_seq); - error_count++; - } - if (message_lengths[last_msg.msgid] != last_msg.len) { - printf("Incorrect message length %u for message %u - expected %u\n", - (unsigned)last_msg.len, (unsigned)last_msg.msgid, message_lengths[last_msg.msgid]); - error_count++; - } - } - if (status.packet_rx_drop_count != 0) { - printf("Parse error at packet %u\n", chan_counts[chan]); - error_count++; - } -} - -int main(void) -{ - mavlink_channel_t chan; - mavlink_test_all(11, 10, &last_msg); - for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) { - printf("Received %u messages on channel %u OK\n", - chan_counts[chan], (unsigned)chan); - } - if (error_count != 0) { - printf("Error count %u\n", error_count); - exit(1); - } - printf("No errors detected\n"); - return 0; -} - diff --git a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.cpp b/mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.cpp deleted file mode 100644 index 98b4abf05..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.cpp +++ /dev/null @@ -1,8 +0,0 @@ -// stdafx.cpp : source file that includes just the standard includes -// testmav.pch will be the pre-compiled header -// stdafx.obj will contain the pre-compiled type information - -#include "stdafx.h" - -// TODO: reference any additional headers you need in STDAFX.H -// and not in this file diff --git a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.h b/mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.h deleted file mode 100644 index 47a0d0252..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.h +++ /dev/null @@ -1,15 +0,0 @@ -// stdafx.h : include file for standard system include files, -// or project specific include files that are used frequently, but -// are changed infrequently -// - -#pragma once - -#include "targetver.h" - -#include -#include - - - -// TODO: reference additional headers your program requires here diff --git a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/targetver.h b/mavlink/share/pyshared/pymavlink/generator/C/test/windows/targetver.h deleted file mode 100644 index 90e767bfc..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/targetver.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -// Including SDKDDKVer.h defines the highest available Windows platform. - -// If you wish to build your application for a previous Windows platform, include WinSDKVer.h and -// set the _WIN32_WINNT macro to the platform you wish to support before including SDKDDKVer.h. - -#include diff --git a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/testmav.cpp b/mavlink/share/pyshared/pymavlink/generator/C/test/windows/testmav.cpp deleted file mode 100644 index aa83caced..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/testmav.cpp +++ /dev/null @@ -1,154 +0,0 @@ -// testmav.cpp : Defines the entry point for the console application. -// - -#include "stdafx.h" -#include "stdio.h" -#include "stdint.h" -#include "stddef.h" -#include "assert.h" - - -#define MAVLINK_USE_CONVENIENCE_FUNCTIONS -#define MAVLINK_COMM_NUM_BUFFERS 2 - -#include -static mavlink_system_t mavlink_system = {42,11,}; - -#define MAVLINK_ASSERT(x) assert(x) -static void comm_send_ch(mavlink_channel_t chan, uint8_t c); - -static mavlink_message_t last_msg; - -#include -#include - -static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS]; - -static const unsigned message_lengths[] = MAVLINK_MESSAGE_LENGTHS; -static unsigned error_count; - -static const mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO; - -static void print_one_field(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx) -{ -#define PRINT_FORMAT(f, def) (f->print_format?f->print_format:def) - switch (f->type) { - case MAVLINK_TYPE_CHAR: - printf(PRINT_FORMAT(f, "%c"), _MAV_RETURN_char(msg, f->wire_offset+idx*1)); - break; - case MAVLINK_TYPE_UINT8_T: - printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint8_t(msg, f->wire_offset+idx*1)); - break; - case MAVLINK_TYPE_INT8_T: - printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int8_t(msg, f->wire_offset+idx*1)); - break; - case MAVLINK_TYPE_UINT16_T: - printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint16_t(msg, f->wire_offset+idx*2)); - break; - case MAVLINK_TYPE_INT16_T: - printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int16_t(msg, f->wire_offset+idx*2)); - break; - case MAVLINK_TYPE_UINT32_T: - printf(PRINT_FORMAT(f, "%lu"), (unsigned long)_MAV_RETURN_uint32_t(msg, f->wire_offset+idx*4)); - break; - case MAVLINK_TYPE_INT32_T: - printf(PRINT_FORMAT(f, "%ld"), (long)_MAV_RETURN_int32_t(msg, f->wire_offset+idx*4)); - break; - case MAVLINK_TYPE_UINT64_T: - printf(PRINT_FORMAT(f, "%llu"), (unsigned long long)_MAV_RETURN_uint64_t(msg, f->wire_offset+idx*8)); - break; - case MAVLINK_TYPE_INT64_T: - printf(PRINT_FORMAT(f, "%lld"), (long long)_MAV_RETURN_int64_t(msg, f->wire_offset+idx*8)); - break; - case MAVLINK_TYPE_FLOAT: - printf(PRINT_FORMAT(f, "%f"), (double)_MAV_RETURN_float(msg, f->wire_offset+idx*4)); - break; - case MAVLINK_TYPE_DOUBLE: - printf(PRINT_FORMAT(f, "%f"), _MAV_RETURN_double(msg, f->wire_offset+idx*8)); - break; - } -} - -static void print_field(mavlink_message_t *msg, const mavlink_field_info_t *f) -{ - printf("%s: ", f->name); - if (f->array_length == 0) { - print_one_field(msg, f, 0); - printf(" "); - } else { - unsigned i; - /* print an array */ - if (f->type == MAVLINK_TYPE_CHAR) { - printf("'%.*s'", f->array_length, - f->wire_offset+(const char *)_MAV_PAYLOAD(msg)); - - } else { - printf("[ "); - for (i=0; iarray_length; i++) { - print_one_field(msg, f, i); - if (i < f->array_length) { - printf(", "); - } - } - printf("]"); - } - } - printf(" "); -} - -static void print_message(mavlink_message_t *msg) -{ - const mavlink_message_info_t *m = &message_info[msg->msgid]; - const mavlink_field_info_t *f = m->fields; - unsigned i; - printf("%s { ", m->name); - for (i=0; inum_fields; i++) { - print_field(msg, &f[i]); - } - printf("}\n"); -} - -static void comm_send_ch(mavlink_channel_t chan, uint8_t c) -{ - mavlink_status_t status; - if (mavlink_parse_char(chan, c, &last_msg, &status)) { - print_message(&last_msg); - chan_counts[chan]++; - /* channel 0 gets 3 messages per message, because of - the channel defaults for _pack() and _encode() */ - if (chan == MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)(chan_counts[chan]*3)) { - printf("Channel 0 sequence mismatch error at packet %u (rx_seq=%u)\n", - chan_counts[chan], status.current_rx_seq); - error_count++; - } else if (chan > MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)chan_counts[chan]) { - printf("Channel %u sequence mismatch error at packet %u (rx_seq=%u)\n", - (unsigned)chan, chan_counts[chan], status.current_rx_seq); - error_count++; - } - if (message_lengths[last_msg.msgid] != last_msg.len) { - printf("Incorrect message length %u for message %u - expected %u\n", - (unsigned)last_msg.len, (unsigned)last_msg.msgid, message_lengths[last_msg.msgid]); - error_count++; - } - } - if (status.packet_rx_drop_count != 0) { - printf("Parse error at packet %u\n", chan_counts[chan]); - error_count++; - } -} - -int _tmain(int argc, _TCHAR* argv[]) -{ - int chan; - mavlink_test_all(11, 10, &last_msg); - for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) { - printf("Received %u messages on channel %u OK\n", - chan_counts[chan], (unsigned)chan); - } - if (error_count != 0) { - printf("Error count %u\n", error_count); - return(1); - } - printf("No errors detected\n"); - return 0; -} diff --git a/mavlink/share/pyshared/pymavlink/generator/gen_MatrixPilot.py b/mavlink/share/pyshared/pymavlink/generator/gen_MatrixPilot.py deleted file mode 100644 index 165c1b343..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/gen_MatrixPilot.py +++ /dev/null @@ -1,93 +0,0 @@ -#!/usr/bin/env python -''' -Use mavgen.py matrixpilot.xml definitions to generate -C and Python MAVLink routines for sending and parsing the protocol -This python script is soley for MatrixPilot MAVLink impoementation - -Copyright Pete Hollands 2011 -Released under GNU GPL version 3 or later -''' - -import os, sys, glob, re -from shutil import copy -from mavgen import mavgen - -# allow import from the parent directory, where mavutil.py is -# Under Windows, this script must be run from a DOS command window -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -class options: - """ a class to simulate the options of mavgen OptionsParser""" - def __init__(self, lang, output, wire_protocol): - self.language = lang - self.wire_protocol = wire_protocol - self.output = output - -def remove_include_files(target_directory): - search_pattern = target_directory+'/*.h' - print "search pattern is", search_pattern - files_to_remove = glob.glob(search_pattern) - for afile in files_to_remove : - try: - print "removing", afile - os.remove(afile) - except: - print "error while trying to remove", afile - -def copy_include_files(source_directory,target_directory): - search_pattern = source_directory+'/*.h' - files_to_copy = glob.glob(search_pattern) - for afile in files_to_copy: - basename = os.path.basename(afile) - print "Copying ...", basename - copy(afile, target_directory) - -protocol = "1.0" - -xml_directory = './message_definitions/v'+protocol -print "xml_directory is", xml_directory -xml_file_names = [] -xml_file_names.append(xml_directory+"/"+"matrixpilot.xml") - -for xml_file in xml_file_names: - print "xml file is ", xml_file - opts = options(lang = "C", output = "C/include_v"+protocol, \ - wire_protocol=protocol) - args = [] - args.append(xml_file) - mavgen(opts, args) - xml_file_base = os.path.basename(xml_file) - xml_file_base = re.sub("\.xml","", xml_file_base) - print "xml_file_base is", xml_file_base - opts = options(lang = "python", \ - output="python/mavlink_"+xml_file_base+"_v"+protocol+".py", \ - wire_protocol=protocol) - mavgen(opts,args) - -mavlink_directory_list = ["common","matrixpilot"] -for mavlink_directory in mavlink_directory_list : - # Look specifically for MatrixPilot directory structure - target_directory = "../../../../MAVLink/include/"+mavlink_directory - source_directory = "C/include_v"+protocol+"/"+mavlink_directory - if os.access(source_directory, os.R_OK): - if os.access(target_directory, os.W_OK): - print "Preparing to copy over files..." - print "About to remove all files in",target_directory - print "OK to continue ?[Yes / No]: ", - line = sys.stdin.readline() - if line == "Yes\n" or line == "yes\n" \ - or line == "Y\n" or line == "y\n": - print "passed" - remove_include_files(target_directory) - copy_include_files(source_directory,target_directory) - print "Finished copying over include files" - else : - print "Your answer is No. Exiting Program" - sys.exit() - else : - print "Cannot find " + target_directory + "in MatrixPilot" - sys.exit() - else: - print "Could not find files to copy at", source_directory - print "Exiting Program." - sys.exit() diff --git a/mavlink/share/pyshared/pymavlink/generator/gen_all.py b/mavlink/share/pyshared/pymavlink/generator/gen_all.py deleted file mode 100644 index 5b24f85cb..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/gen_all.py +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env python - -''' -Use mavgen.py on all available MAVLink XML definitions to generate -C and Python MAVLink routines for sending and parsing the protocol - -Copyright Pete Hollands 2011 -Released under GNU GPL version 3 or later -''' - -import os, sys, glob, re -from mavgen import mavgen - -# allow import from the parent directory, where mavutil.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -class options: - """ a class to simulate the options of mavgen OptionsParser""" - def __init__(self, lang, output, wire_protocol): - self.language = lang - self.wire_protocol = wire_protocol - self.output = output - -protocols = [ '0.9', '1.0' ] - -for protocol in protocols : - xml_directory = './message_definitions/v'+protocol - print "xml_directory is", xml_directory - xml_file_names = glob.glob(xml_directory+'/*.xml') - - for xml_file in xml_file_names: - print "xml file is ", xml_file - opts = options(lang = "C", output = "C/include_v"+protocol, \ - wire_protocol=protocol) - args = [] - args.append(xml_file) - mavgen(opts, args) - xml_file_base = os.path.basename(xml_file) - xml_file_base = re.sub("\.xml","", xml_file_base) - print "xml_file_base is", xml_file_base - opts = options(lang = "python", \ - output="python/mavlink_"+xml_file_base+"_v"+protocol+".py", \ - wire_protocol=protocol) - mavgen(opts,args) diff --git a/mavlink/share/pyshared/pymavlink/generator/gen_all.sh b/mavlink/share/pyshared/pymavlink/generator/gen_all.sh deleted file mode 100644 index e8dafedc5..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/gen_all.sh +++ /dev/null @@ -1,12 +0,0 @@ -#!/bin/sh - -for protocol in 0.9 1.0; do - for xml in message_definitions/v$protocol/*.xml; do - base=$(basename $xml .xml) - ./mavgen.py --lang=C --wire-protocol=$protocol --output=C/include_v$protocol $xml || exit 1 - ./mavgen.py --lang=python --wire-protocol=$protocol --output=python/mavlink_${base}_v$protocol.py $xml || exit 1 - done -done - -cp -f python/mavlink_ardupilotmega_v0.9.py ../mavlink.py -cp -f python/mavlink_ardupilotmega_v1.0.py ../mavlinkv10.py diff --git a/mavlink/share/pyshared/pymavlink/generator/mavgen.py b/mavlink/share/pyshared/pymavlink/generator/mavgen.py deleted file mode 100644 index 05f71f778..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/mavgen.py +++ /dev/null @@ -1,82 +0,0 @@ -#!/usr/bin/env python -''' -parse a MAVLink protocol XML file and generate a python implementation - -Copyright Andrew Tridgell 2011 -Released under GNU GPL version 3 or later -''' - -def mavgen(opts, args) : - """Generate mavlink message formatters and parsers (C and Python ) using options - and args where args are a list of xml files. This function allows python - scripts under Windows to control mavgen using the same interface as - shell scripts under Unix""" - import sys, textwrap, os - - import mavparse - import mavgen_python - import mavgen_c - - xml = [] - - for fname in args: - print("Parsing %s" % fname) - xml.append(mavparse.MAVXML(fname, opts.wire_protocol)) - - # expand includes - for x in xml[:]: - for i in x.include: - fname = os.path.join(os.path.dirname(x.filename), i) - print("Parsing %s" % fname) - xml.append(mavparse.MAVXML(fname, opts.wire_protocol)) - - # include message lengths and CRCs too - for idx in range(0, 256): - if x.message_lengths[idx] == 0: - x.message_lengths[idx] = xml[-1].message_lengths[idx] - x.message_crcs[idx] = xml[-1].message_crcs[idx] - x.message_names[idx] = xml[-1].message_names[idx] - - # work out max payload size across all includes - largest_payload = 0 - for x in xml: - if x.largest_payload > largest_payload: - largest_payload = x.largest_payload - for x in xml: - x.largest_payload = largest_payload - - if mavparse.check_duplicates(xml): - sys.exit(1) - - print("Found %u MAVLink message types in %u XML files" % ( - mavparse.total_msgs(xml), len(xml))) - - if opts.language == 'python': - mavgen_python.generate(opts.output, xml) - elif opts.language == 'C': - mavgen_c.generate(opts.output, xml) - else: - print("Unsupported language %s" % opts.language) - - -if __name__=="__main__": - import sys, textwrap, os - - from optparse import OptionParser - - # allow import from the parent directory, where mavutil.py is - sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - - import mavparse - import mavgen_python - import mavgen_c - - parser = OptionParser("%prog [options] ") - parser.add_option("-o", "--output", dest="output", default="mavlink", help="output directory.") - parser.add_option("--lang", dest="language", default="python", help="language of generated code: 'Python' or 'C' [default: %default]") - parser.add_option("--wire-protocol", dest="wire_protocol", default=mavparse.PROTOCOL_0_9, help="MAVLink protocol version: '0.9' or '1.0'. [default: %default]") - (opts, args) = parser.parse_args() - - if len(args) < 1: - parser.error("You must supply at least one MAVLink XML protocol definition") - mavgen(opts, args) diff --git a/mavlink/share/pyshared/pymavlink/generator/mavgen_c.py b/mavlink/share/pyshared/pymavlink/generator/mavgen_c.py deleted file mode 100644 index 255919f0d..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/mavgen_c.py +++ /dev/null @@ -1,581 +0,0 @@ -#!/usr/bin/env python -''' -parse a MAVLink protocol XML file and generate a C implementation - -Copyright Andrew Tridgell 2011 -Released under GNU GPL version 3 or later -''' - -import sys, textwrap, os, time -import mavparse, mavtemplate - -t = mavtemplate.MAVTemplate() - -def generate_version_h(directory, xml): - '''generate version.h''' - f = open(os.path.join(directory, "version.h"), mode='w') - t.write(f,''' -/** @file - * @brief MAVLink comm protocol built from ${basename}.xml - * @see http://pixhawk.ethz.ch/software/mavlink - */ -#ifndef MAVLINK_VERSION_H -#define MAVLINK_VERSION_H - -#define MAVLINK_BUILD_DATE "${parse_time}" -#define MAVLINK_WIRE_PROTOCOL_VERSION "${wire_protocol_version}" -#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE ${largest_payload} - -#endif // MAVLINK_VERSION_H -''', xml) - f.close() - -def generate_mavlink_h(directory, xml): - '''generate mavlink.h''' - f = open(os.path.join(directory, "mavlink.h"), mode='w') - t.write(f,''' -/** @file - * @brief MAVLink comm protocol built from ${basename}.xml - * @see http://pixhawk.ethz.ch/software/mavlink - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#ifndef MAVLINK_STX -#define MAVLINK_STX ${protocol_marker} -#endif - -#ifndef MAVLINK_ENDIAN -#define MAVLINK_ENDIAN ${mavlink_endian} -#endif - -#ifndef MAVLINK_ALIGNED_FIELDS -#define MAVLINK_ALIGNED_FIELDS ${aligned_fields_define} -#endif - -#ifndef MAVLINK_CRC_EXTRA -#define MAVLINK_CRC_EXTRA ${crc_extra_define} -#endif - -#include "version.h" -#include "${basename}.h" - -#endif // MAVLINK_H -''', xml) - f.close() - -def generate_main_h(directory, xml): - '''generate main header per XML file''' - f = open(os.path.join(directory, xml.basename + ".h"), mode='w') - t.write(f, ''' -/** @file - * @brief MAVLink comm protocol generated from ${basename}.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef ${basename_upper}_H -#define ${basename_upper}_H - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {${message_lengths_array}} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {${message_crcs_array}} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {${message_info_array}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_${basename_upper} - -${{include_list:#include "../${base}/${base}.h" -}} - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION ${version} -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION ${version} -#endif - -// ENUM DEFINITIONS - -${{enum: -/** @brief ${description} */ -#ifndef HAVE_ENUM_${name} -#define HAVE_ENUM_${name} -enum ${name} -{ -${{entry: ${name}=${value}, /* ${description} |${{param:${description}| }} */ -}} -}; -#endif -}} - -// MESSAGE DEFINITIONS -${{message:#include "./mavlink_msg_${name_lower}.h" -}} - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // ${basename_upper}_H -''', xml) - - f.close() - - -def generate_message_h(directory, m): - '''generate per-message header for a XML file''' - f = open(os.path.join(directory, 'mavlink_msg_%s.h' % m.name_lower), mode='w') - t.write(f, ''' -// MESSAGE ${name} PACKING - -#define MAVLINK_MSG_ID_${name} ${id} - -typedef struct __mavlink_${name_lower}_t -{ -${{ordered_fields: ${type} ${name}${array_suffix}; ///< ${description} -}} -} mavlink_${name_lower}_t; - -#define MAVLINK_MSG_ID_${name}_LEN ${wire_length} -#define MAVLINK_MSG_ID_${id}_LEN ${wire_length} - -${{array_fields:#define MAVLINK_MSG_${msg_name}_FIELD_${name_upper}_LEN ${array_length} -}} - -#define MAVLINK_MESSAGE_INFO_${name} { \\ - "${name}", \\ - ${num_fields}, \\ - { ${{ordered_fields: { "${name}", ${c_print_format}, MAVLINK_TYPE_${type_upper}, ${array_length}, ${wire_offset}, offsetof(mavlink_${name_lower}_t, ${name}) }, \\ - }} } \\ -} - - -/** - * @brief Pack a ${name_lower} message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * -${{arg_fields: * @param ${name} ${description} -}} - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_${name_lower}_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - ${{arg_fields: ${array_const}${type} ${array_prefix}${name},}}) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[${wire_length}]; -${{scalar_fields: _mav_put_${type}(buf, ${wire_offset}, ${putname}); -}} -${{array_fields: _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length}); -}} - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, ${wire_length}); -#else - mavlink_${name_lower}_t packet; -${{scalar_fields: packet.${name} = ${putname}; -}} -${{array_fields: mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length}); -}} - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, ${wire_length}); -#endif - - msg->msgid = MAVLINK_MSG_ID_${name}; - return mavlink_finalize_message(msg, system_id, component_id, ${wire_length}${crc_extra_arg}); -} - -/** - * @brief Pack a ${name_lower} message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into -${{arg_fields: * @param ${name} ${description} -}} - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_${name_lower}_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - ${{arg_fields:${array_const}${type} ${array_prefix}${name},}}) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[${wire_length}]; -${{scalar_fields: _mav_put_${type}(buf, ${wire_offset}, ${putname}); -}} -${{array_fields: _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length}); -}} - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, ${wire_length}); -#else - mavlink_${name_lower}_t packet; -${{scalar_fields: packet.${name} = ${putname}; -}} -${{array_fields: mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length}); -}} - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, ${wire_length}); -#endif - - msg->msgid = MAVLINK_MSG_ID_${name}; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, ${wire_length}${crc_extra_arg}); -} - -/** - * @brief Encode a ${name_lower} struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param ${name_lower} C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_${name_lower}_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_${name_lower}_t* ${name_lower}) -{ - return mavlink_msg_${name_lower}_pack(system_id, component_id, msg,${{arg_fields: ${name_lower}->${name},}}); -} - -/** - * @brief Send a ${name_lower} message - * @param chan MAVLink channel to send the message - * -${{arg_fields: * @param ${name} ${description} -}} - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_${name_lower}_send(mavlink_channel_t chan,${{arg_fields: ${array_const}${type} ${array_prefix}${name},}}) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[${wire_length}]; -${{scalar_fields: _mav_put_${type}(buf, ${wire_offset}, ${putname}); -}} -${{array_fields: _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length}); -}} - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, buf, ${wire_length}${crc_extra_arg}); -#else - mavlink_${name_lower}_t packet; -${{scalar_fields: packet.${name} = ${putname}; -}} -${{array_fields: mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length}); -}} - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, (const char *)&packet, ${wire_length}${crc_extra_arg}); -#endif -} - -#endif - -// MESSAGE ${name} UNPACKING - -${{fields: -/** - * @brief Get field ${name} from ${name_lower} message - * - * @return ${description} - */ -static inline ${return_type} mavlink_msg_${name_lower}_get_${name}(const mavlink_message_t* msg${get_arg}) -{ - return _MAV_RETURN_${type}${array_tag}(msg, ${array_return_arg} ${wire_offset}); -} -}} - -/** - * @brief Decode a ${name_lower} message into a struct - * - * @param msg The message to decode - * @param ${name_lower} C-struct to decode the message contents into - */ -static inline void mavlink_msg_${name_lower}_decode(const mavlink_message_t* msg, mavlink_${name_lower}_t* ${name_lower}) -{ -#if MAVLINK_NEED_BYTE_SWAP -${{ordered_fields: ${decode_left}mavlink_msg_${name_lower}_get_${name}(msg${decode_right}); -}} -#else - memcpy(${name_lower}, _MAV_PAYLOAD(msg), ${wire_length}); -#endif -} -''', m) - f.close() - - -def generate_testsuite_h(directory, xml): - '''generate testsuite.h per XML file''' - f = open(os.path.join(directory, "testsuite.h"), mode='w') - t.write(f, ''' -/** @file - * @brief MAVLink comm protocol testsuite generated from ${basename}.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef ${basename_upper}_TESTSUITE_H -#define ${basename_upper}_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -${{include_list:static void mavlink_test_${base}(uint8_t, uint8_t, mavlink_message_t *last_msg); -}} -static void mavlink_test_${basename}(uint8_t, uint8_t, mavlink_message_t *last_msg); - -static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ -${{include_list: mavlink_test_${base}(system_id, component_id, last_msg); -}} - mavlink_test_${basename}(system_id, component_id, last_msg); -} -#endif - -${{include_list:#include "../${base}/testsuite.h" -}} - -${{message: -static void mavlink_test_${name_lower}(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_${name_lower}_t packet_in = { - ${{ordered_fields:${c_test_value}, - }}}; - mavlink_${name_lower}_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - ${{scalar_fields: packet1.${name} = packet_in.${name}; - }} - ${{array_fields: mav_array_memcpy(packet1.${name}, packet_in.${name}, sizeof(${type})*${array_length}); - }} - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_${name_lower}_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_${name_lower}_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_${name_lower}_pack(system_id, component_id, &msg ${{arg_fields:, packet1.${name} }}); - mavlink_msg_${name_lower}_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_${name_lower}_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg ${{arg_fields:, packet1.${name} }}); - mavlink_msg_${name_lower}_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; i= 1 and self.buf[0] != ${protocol_marker}: - magic = self.buf[0] - self.buf = self.buf[1:] - if self.robust_parsing: - m = MAVLink_bad_data(chr(magic), "Bad prefix") - if self.callback: - self.callback(m, *self.callback_args, **self.callback_kwargs) - self.expected_length = 6 - self.total_receive_errors += 1 - return m - if self.have_prefix_error: - return None - self.have_prefix_error = True - self.total_receive_errors += 1 - raise MAVError("invalid MAVLink prefix '%s'" % magic) - self.have_prefix_error = False - if len(self.buf) >= 2: - (magic, self.expected_length) = struct.unpack('BB', self.buf[0:2]) - self.expected_length += 8 - if self.expected_length >= 8 and len(self.buf) >= self.expected_length: - mbuf = self.buf[0:self.expected_length] - self.buf = self.buf[self.expected_length:] - self.expected_length = 6 - if self.robust_parsing: - try: - m = self.decode(mbuf) - self.total_packets_received += 1 - except MAVError as reason: - m = MAVLink_bad_data(mbuf, reason.message) - self.total_receive_errors += 1 - else: - m = self.decode(mbuf) - self.total_packets_received += 1 - if self.callback: - self.callback(m, *self.callback_args, **self.callback_kwargs) - return m - return None - - def parse_buffer(self, s): - '''input some data bytes, possibly returning a list of new messages''' - m = self.parse_char(s) - if m is None: - return None - ret = [m] - while True: - m = self.parse_char("") - if m is None: - return ret - ret.append(m) - return ret - - def decode(self, msgbuf): - '''decode a buffer as a MAVLink message''' - # decode the header - try: - magic, mlen, seq, srcSystem, srcComponent, msgId = struct.unpack('cBBBBB', msgbuf[:6]) - except struct.error as emsg: - raise MAVError('Unable to unpack MAVLink header: %s' % emsg) - if ord(magic) != ${protocol_marker}: - raise MAVError("invalid MAVLink prefix '%s'" % magic) - if mlen != len(msgbuf)-8: - raise MAVError('invalid MAVLink message length. Got %u expected %u, msgId=%u' % (len(msgbuf)-8, mlen, msgId)) - - if not msgId in mavlink_map: - raise MAVError('unknown MAVLink message ID %u' % msgId) - - # decode the payload - (fmt, type, order_map, crc_extra) = mavlink_map[msgId] - - # decode the checksum - try: - crc, = struct.unpack(' self.enum[-1].highest_value): - self.enum[-1].highest_value = value - self.enum[-1].entry.append(MAVEnumEntry(attrs['name'], value)) - elif in_element == "mavlink.enums.enum.entry.param": - check_attrs(attrs, ['index'], 'enum param') - self.enum[-1].entry[-1].param.append(MAVEnumParam(attrs['index'])) - - def end_element(name): - in_element = '.'.join(in_element_list) - if in_element == "mavlink.enums.enum": - # add a ENUM_END - self.enum[-1].entry.append(MAVEnumEntry("%s_ENUM_END" % self.enum[-1].name, - self.enum[-1].highest_value+1, end_marker=True)) - in_element_list.pop() - - def char_data(data): - in_element = '.'.join(in_element_list) - if in_element == "mavlink.messages.message.description": - self.message[-1].description += data - elif in_element == "mavlink.messages.message.field": - self.message[-1].fields[-1].description += data - elif in_element == "mavlink.enums.enum.description": - self.enum[-1].description += data - elif in_element == "mavlink.enums.enum.entry.description": - self.enum[-1].entry[-1].description += data - elif in_element == "mavlink.enums.enum.entry.param": - self.enum[-1].entry[-1].param[-1].description += data - elif in_element == "mavlink.version": - self.version = int(data) - elif in_element == "mavlink.include": - self.include.append(data) - - f = open(filename, mode='rb') - p = xml.parsers.expat.ParserCreate() - p.StartElementHandler = start_element - p.EndElementHandler = end_element - p.CharacterDataHandler = char_data - p.ParseFile(f) - f.close() - - self.message_lengths = [ 0 ] * 256 - self.message_crcs = [ 0 ] * 256 - self.message_names = [ None ] * 256 - self.largest_payload = 0 - - for m in self.message: - m.wire_length = 0 - m.fieldnames = [] - m.ordered_fieldnames = [] - if self.sort_fields: - m.ordered_fields = sorted(m.fields, - key=operator.attrgetter('type_length'), - reverse=True) - else: - m.ordered_fields = m.fields - for f in m.fields: - m.fieldnames.append(f.name) - for f in m.ordered_fields: - f.wire_offset = m.wire_length - m.wire_length += f.wire_length - m.ordered_fieldnames.append(f.name) - f.set_test_value() - m.num_fields = len(m.fieldnames) - if m.num_fields > 64: - raise MAVParseError("num_fields=%u : Maximum number of field names allowed is" % ( - m.num_fields, 64)) - m.crc_extra = message_checksum(m) - self.message_lengths[m.id] = m.wire_length - self.message_names[m.id] = m.name - self.message_crcs[m.id] = m.crc_extra - if m.wire_length > self.largest_payload: - self.largest_payload = m.wire_length - - if m.wire_length+8 > 64: - print("Note: message %s is longer than 64 bytes long (%u bytes), which can cause fragmentation since many radio modems use 64 bytes as maximum air transfer unit." % (m.name, m.wire_length+8)) - - def __str__(self): - return "MAVXML for %s from %s (%u message, %u enums)" % ( - self.basename, self.filename, len(self.message), len(self.enum)) - - -def message_checksum(msg): - '''calculate a 8-bit checksum of the key fields of a message, so we - can detect incompatible XML changes''' - crc = mavutil.x25crc(msg.name + ' ') - for f in msg.ordered_fields: - crc.accumulate(f.type + ' ') - crc.accumulate(f.name + ' ') - if f.array_length: - crc.accumulate(chr(f.array_length)) - return (crc.crc&0xFF) ^ (crc.crc>>8) - -def merge_enums(xml): - '''merge enums between XML files''' - emap = {} - for x in xml: - newenums = [] - for enum in x.enum: - if enum.name in emap: - emap[enum.name].entry.pop() # remove end marker - emap[enum.name].entry.extend(enum.entry) - print("Merged enum %s" % enum.name) - else: - newenums.append(enum) - emap[enum.name] = enum - x.enum = newenums - # sort by value - for e in emap: - emap[e].entry = sorted(emap[e].entry, - key=operator.attrgetter('value'), - reverse=False) - - -def check_duplicates(xml): - '''check for duplicate message IDs''' - - merge_enums(xml) - - msgmap = {} - enummap = {} - for x in xml: - for m in x.message: - if m.id in msgmap: - print("ERROR: Duplicate message id %u for %s (%s:%u) also used by %s" % ( - m.id, m.name, - x.filename, m.linenumber, - msgmap[m.id])) - return True - fieldset = set() - for f in m.fields: - if f.name in fieldset: - print("ERROR: Duplicate field %s in message %s (%s:%u)" % ( - f.name, m.name, - x.filename, m.linenumber)) - return True - fieldset.add(f.name) - msgmap[m.id] = '%s (%s:%u)' % (m.name, x.filename, m.linenumber) - for enum in x.enum: - for entry in enum.entry: - s1 = "%s.%s" % (enum.name, entry.name) - s2 = "%s.%s" % (enum.name, entry.value) - if s1 in enummap or s2 in enummap: - print("ERROR: Duplicate enums %s/%s at %s:%u and %s" % ( - s1, entry.value, x.filename, enum.linenumber, - enummap.get(s1) or enummap.get(s2))) - return True - enummap[s1] = "%s:%u" % (x.filename, enum.linenumber) - enummap[s2] = "%s:%u" % (x.filename, enum.linenumber) - - return False - - - -def total_msgs(xml): - '''count total number of msgs''' - count = 0 - for x in xml: - count += len(x.message) - return count - -def mkdir_p(dir): - try: - os.makedirs(dir) - except OSError as exc: - if exc.errno == errno.EEXIST: - pass - else: raise - -# check version consistent -# add test.xml -# finish test suite -# printf style error macro, if defined call errors diff --git a/mavlink/share/pyshared/pymavlink/generator/mavtemplate.py b/mavlink/share/pyshared/pymavlink/generator/mavtemplate.py deleted file mode 100644 index 6ef015315..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/mavtemplate.py +++ /dev/null @@ -1,121 +0,0 @@ -#!/usr/bin/env python -''' -simple templating system for mavlink generator - -Copyright Andrew Tridgell 2011 -Released under GNU GPL version 3 or later -''' - -from mavparse import MAVParseError - -class MAVTemplate(object): - '''simple templating system''' - def __init__(self, - start_var_token="${", - end_var_token="}", - start_rep_token="${{", - end_rep_token="}}", - trim_leading_lf=True, - checkmissing=True): - self.start_var_token = start_var_token - self.end_var_token = end_var_token - self.start_rep_token = start_rep_token - self.end_rep_token = end_rep_token - self.trim_leading_lf = trim_leading_lf - self.checkmissing = checkmissing - - def find_end(self, text, start_token, end_token): - '''find the of a token. - Returns the offset in the string immediately after the matching end_token''' - if not text.startswith(start_token): - raise MAVParseError("invalid token start") - offset = len(start_token) - nesting = 1 - while nesting > 0: - idx1 = text[offset:].find(start_token) - idx2 = text[offset:].find(end_token) - if idx1 == -1 and idx2 == -1: - raise MAVParseError("token nesting error") - if idx1 == -1 or idx1 > idx2: - offset += idx2 + len(end_token) - nesting -= 1 - else: - offset += idx1 + len(start_token) - nesting += 1 - return offset - - def find_var_end(self, text): - '''find the of a variable''' - return self.find_end(text, self.start_var_token, self.end_var_token) - - def find_rep_end(self, text): - '''find the of a repitition''' - return self.find_end(text, self.start_rep_token, self.end_rep_token) - - def substitute(self, text, subvars={}, - trim_leading_lf=None, checkmissing=None): - '''substitute variables in a string''' - - if trim_leading_lf is None: - trim_leading_lf = self.trim_leading_lf - if checkmissing is None: - checkmissing = self.checkmissing - - # handle repititions - while True: - subidx = text.find(self.start_rep_token) - if subidx == -1: - break - endidx = self.find_rep_end(text[subidx:]) - if endidx == -1: - raise MAVParseError("missing end macro in %s" % text[subidx:]) - part1 = text[0:subidx] - part2 = text[subidx+len(self.start_rep_token):subidx+(endidx-len(self.end_rep_token))] - part3 = text[subidx+endidx:] - a = part2.split(':') - field_name = a[0] - rest = ':'.join(a[1:]) - v = getattr(subvars, field_name, None) - if v is None: - raise MAVParseError('unable to find field %s' % field_name) - t1 = part1 - for f in v: - t1 += self.substitute(rest, f, trim_leading_lf=False, checkmissing=False) - if len(v) != 0 and t1[-1] in ["\n", ","]: - t1 = t1[:-1] - t1 += part3 - text = t1 - - if trim_leading_lf: - if text[0] == '\n': - text = text[1:] - while True: - idx = text.find(self.start_var_token) - if idx == -1: - return text - endidx = text[idx:].find(self.end_var_token) - if endidx == -1: - raise MAVParseError('missing end of variable: %s' % text[idx:idx+10]) - varname = text[idx+2:idx+endidx] - if isinstance(subvars, dict): - if not varname in subvars: - if checkmissing: - raise MAVParseError("unknown variable in '%s%s%s'" % ( - self.start_var_token, varname, self.end_var_token)) - return text[0:idx+endidx] + self.substitute(text[idx+endidx:], subvars, - trim_leading_lf=False, checkmissing=False) - value = subvars[varname] - else: - value = getattr(subvars, varname, None) - if value is None: - if checkmissing: - raise MAVParseError("unknown variable in '%s%s%s'" % ( - self.start_var_token, varname, self.end_var_token)) - return text[0:idx+endidx] + self.substitute(text[idx+endidx:], subvars, - trim_leading_lf=False, checkmissing=False) - text = text.replace("%s%s%s" % (self.start_var_token, varname, self.end_var_token), str(value)) - return text - - def write(self, file, text, subvars={}, trim_leading_lf=True): - '''write to a file with variable substitution''' - file.write(self.substitute(text, subvars=subvars, trim_leading_lf=trim_leading_lf)) diff --git a/mavlink/share/pyshared/pymavlink/generator/mavtestgen.py b/mavlink/share/pyshared/pymavlink/generator/mavtestgen.py deleted file mode 100644 index faffa1c19..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/mavtestgen.py +++ /dev/null @@ -1,142 +0,0 @@ -#!/usr/bin/env python -''' -generate a MAVLink test suite - -Copyright Andrew Tridgell 2011 -Released under GNU GPL version 3 or later -''' - -import sys, textwrap -from optparse import OptionParser - -# mavparse is up a directory level -sys.path.append('..') -import mavparse - -def gen_value(f, i, language): - '''generate a test value for the ith field of a message''' - type = f.type - - # could be an array - if type.find("[") != -1: - aidx = type.find("[") - basetype = type[0:aidx] - if basetype == "array": - basetype = "int8_t" - if language == 'C': - return '(const %s *)"%s%u"' % (basetype, f.name, i) - return '"%s%u"' % (f.name, i) - - if type == 'float': - return 17.0 + i*7 - if type == 'char': - return 'A' + i - if type == 'int8_t': - return 5 + i - if type in ['int8_t', 'uint8_t']: - return 5 + i - if type in ['uint8_t_mavlink_version']: - return 2 - if type in ['int16_t', 'uint16_t']: - return 17235 + i*52 - if type in ['int32_t', 'uint32_t']: - v = 963497464 + i*52 - if language == 'C': - return "%sL" % v - return v - if type in ['int64_t', 'uint64_t']: - v = 9223372036854775807 + i*63 - if language == 'C': - return "%sLL" % v - return v - - - -def generate_methods_python(outf, msgs): - outf.write(""" -''' -MAVLink protocol test implementation (auto-generated by mavtestgen.py) - -Generated from: %s - -Note: this file has been auto-generated. DO NOT EDIT -''' - -import mavlink - -def generate_outputs(mav): - '''generate all message types as outputs''' -""") - for m in msgs: - if m.name == "HEARTBEAT": continue - outf.write("\tmav.%s_send(" % m.name.lower()) - for i in range(0, len(m.fields)): - f = m.fields[i] - outf.write("%s=%s" % (f.name, gen_value(f, i, 'py'))) - if i != len(m.fields)-1: - outf.write(",") - outf.write(")\n") - - -def generate_methods_C(outf, msgs): - outf.write(""" -/* -MAVLink protocol test implementation (auto-generated by mavtestgen.py) - -Generated from: %s - -Note: this file has been auto-generated. DO NOT EDIT -*/ - -static void mavtest_generate_outputs(mavlink_channel_t chan) -{ -""") - for m in msgs: - if m.name == "HEARTBEAT": continue - outf.write("\tmavlink_msg_%s_send(chan," % m.name.lower()) - for i in range(0, len(m.fields)): - f = m.fields[i] - outf.write("%s" % gen_value(f, i, 'C')) - if i != len(m.fields)-1: - outf.write(",") - outf.write(");\n") - outf.write("}\n") - - - -###################################################################### -'''main program''' - -parser = OptionParser("%prog [options] ") -parser.add_option("-o", "--output", dest="output", default="mavtest", help="output folder [default: %default]") -(opts, args) = parser.parse_args() - -if len(args) < 1: - parser.error("You must supply at least one MAVLink XML protocol definition") - - -msgs = [] -enums = [] - -for fname in args: - (m, e) = mavparse.parse_mavlink_xml(fname) - msgs.extend(m) - enums.extend(e) - - -if mavparse.check_duplicates(msgs): - sys.exit(1) - -print("Found %u MAVLink message types" % len(msgs)) - -print("Generating python %s" % (opts.output+'.py')) -outf = open(opts.output + '.py', "w") -generate_methods_python(outf, msgs) -outf.close() - -print("Generating C %s" % (opts.output+'.h')) -outf = open(opts.output + '.h', "w") -generate_methods_C(outf, msgs) -outf.close() - -print("Generated %s OK" % opts.output) diff --git a/mavlink/share/pyshared/pymavlink/mavextra.py b/mavlink/share/pyshared/pymavlink/mavextra.py deleted file mode 100644 index 5395f6036..000000000 --- a/mavlink/share/pyshared/pymavlink/mavextra.py +++ /dev/null @@ -1,154 +0,0 @@ -#!/usr/bin/env python -''' -useful extra functions for use by mavlink clients - -Copyright Andrew Tridgell 2011 -Released under GNU GPL version 3 or later -''' - -from math import * - - -def kmh(mps): - '''convert m/s to Km/h''' - return mps*3.6 - -def altitude(press_abs, ground_press=955.0, ground_temp=30): - '''calculate barometric altitude''' - return log(ground_press/press_abs)*(ground_temp+273.15)*29271.267*0.001 - - -def mag_heading(RAW_IMU, ATTITUDE, declination=0, SENSOR_OFFSETS=None, ofs=None): - '''calculate heading from raw magnetometer''' - mag_x = RAW_IMU.xmag - mag_y = RAW_IMU.ymag - mag_z = RAW_IMU.zmag - if SENSOR_OFFSETS is not None and ofs is not None: - mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x - mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y - mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z - - headX = mag_x*cos(ATTITUDE.pitch) + mag_y*sin(ATTITUDE.roll)*sin(ATTITUDE.pitch) + mag_z*cos(ATTITUDE.roll)*sin(ATTITUDE.pitch) - headY = mag_y*cos(ATTITUDE.roll) - mag_z*sin(ATTITUDE.roll) - heading = degrees(atan2(-headY,headX)) + declination - if heading < 0: - heading += 360 - return heading - -def mag_field(RAW_IMU, SENSOR_OFFSETS=None, ofs=None): - '''calculate magnetic field strength from raw magnetometer''' - mag_x = RAW_IMU.xmag - mag_y = RAW_IMU.ymag - mag_z = RAW_IMU.zmag - if SENSOR_OFFSETS is not None and ofs is not None: - mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x - mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y - mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z - return sqrt(mag_x**2 + mag_y**2 + mag_z**2) - -def angle_diff(angle1, angle2): - '''show the difference between two angles in degrees''' - ret = angle1 - angle2 - if ret > 180: - ret -= 360; - if ret < -180: - ret += 360 - return ret - - -lowpass_data = {} - -def lowpass(var, key, factor): - '''a simple lowpass filter''' - global lowpass_data - if not key in lowpass_data: - lowpass_data[key] = var - else: - lowpass_data[key] = factor*lowpass_data[key] + (1.0 - factor)*var - return lowpass_data[key] - -last_delta = {} - -def delta(var, key): - '''calculate slope''' - global last_delta - dv = 0 - if key in last_delta: - dv = var - last_delta[key] - last_delta[key] = var - return dv - -def delta_angle(var, key): - '''calculate slope of an angle''' - global last_delta - dv = 0 - if key in last_delta: - dv = var - last_delta[key] - last_delta[key] = var - if dv > 180: - dv -= 360 - if dv < -180: - dv += 360 - return dv - -def roll_estimate(RAW_IMU,smooth=0.7): - '''estimate roll from accelerometer''' - rx = lowpass(RAW_IMU.xacc,'rx',smooth) - ry = lowpass(RAW_IMU.yacc,'ry',smooth) - rz = lowpass(RAW_IMU.zacc,'rz',smooth) - return degrees(-asin(ry/sqrt(rx**2+ry**2+rz**2))) - -def pitch_estimate(RAW_IMU, smooth=0.7): - '''estimate pitch from accelerometer''' - rx = lowpass(RAW_IMU.xacc,'rx',smooth) - ry = lowpass(RAW_IMU.yacc,'ry',smooth) - rz = lowpass(RAW_IMU.zacc,'rz',smooth) - return degrees(asin(rx/sqrt(rx**2+ry**2+rz**2))) - -def gravity(RAW_IMU, SENSOR_OFFSETS=None, ofs=None, smooth=0.7): - '''estimate pitch from accelerometer''' - rx = RAW_IMU.xacc - ry = RAW_IMU.yacc - rz = RAW_IMU.zacc+45 - if SENSOR_OFFSETS is not None and ofs is not None: - rx += ofs[0] - SENSOR_OFFSETS.accel_cal_x - ry += ofs[1] - SENSOR_OFFSETS.accel_cal_y - rz += ofs[2] - SENSOR_OFFSETS.accel_cal_z - return lowpass(sqrt(rx**2+ry**2+rz**2)*0.01,'_gravity',smooth) - - - -def pitch_sim(SIMSTATE, GPS_RAW): - '''estimate pitch from SIMSTATE accels''' - xacc = SIMSTATE.xacc - lowpass(delta(GPS_RAW.v,"v")*6.6, "v", 0.9) - zacc = SIMSTATE.zacc - zacc += SIMSTATE.ygyro * GPS_RAW.v; - if xacc/zacc >= 1: - return 0 - if xacc/zacc <= -1: - return -0 - return degrees(-asin(xacc/zacc)) - -def distance_two(GPS_RAW1, GPS_RAW2): - '''distance between two points''' - lat1 = radians(GPS_RAW1.lat) - lat2 = radians(GPS_RAW2.lat) - lon1 = radians(GPS_RAW1.lon) - lon2 = radians(GPS_RAW2.lon) - dLat = lat2 - lat1 - dLon = lon2 - lon1 - - a = sin(0.5*dLat) * sin(0.5*dLat) + sin(0.5*dLon) * sin(0.5*dLon) * cos(lat1) * cos(lat2) - c = 2.0 * atan2(sqrt(a), sqrt(1.0-a)) - return 6371 * 1000 * c - - -first_fix = None - -def distance_home(GPS_RAW): - '''distance from first fix point''' - global first_fix - if first_fix == None or first_fix.fix_type < 2: - first_fix = GPS_RAW - return 0 - return distance_two(GPS_RAW, first_fix) diff --git a/mavlink/share/pyshared/pymavlink/mavlink.py b/mavlink/share/pyshared/pymavlink/mavlink.py deleted file mode 100644 index 3287a921d..000000000 --- a/mavlink/share/pyshared/pymavlink/mavlink.py +++ /dev/null @@ -1,4930 +0,0 @@ -''' -MAVLink protocol implementation (auto-generated by mavgen.py) - -Generated from: ardupilotmega.xml,common.xml - -Note: this file has been auto-generated. DO NOT EDIT -''' - -import struct, array, mavutil, time - -WIRE_PROTOCOL_VERSION = "0.9" - -class MAVLink_header(object): - '''MAVLink message header''' - def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0): - self.mlen = mlen - self.seq = seq - self.srcSystem = srcSystem - self.srcComponent = srcComponent - self.msgId = msgId - - def pack(self): - return struct.pack('BBBBBB', 85, self.mlen, self.seq, - self.srcSystem, self.srcComponent, self.msgId) - -class MAVLink_message(object): - '''base MAVLink message class''' - def __init__(self, msgId, name): - self._header = MAVLink_header(msgId) - self._payload = None - self._msgbuf = None - self._crc = None - self._fieldnames = [] - self._type = name - - def get_msgbuf(self): - return self._msgbuf - - def get_header(self): - return self._header - - def get_payload(self): - return self._payload - - def get_crc(self): - return self._crc - - def get_fieldnames(self): - return self._fieldnames - - def get_type(self): - return self._type - - def get_msgId(self): - return self._header.msgId - - def get_srcSystem(self): - return self._header.srcSystem - - def get_srcComponent(self): - return self._header.srcComponent - - def get_seq(self): - return self._header.seq - - def __str__(self): - ret = '%s {' % self._type - for a in self._fieldnames: - v = getattr(self, a) - ret += '%s : %s, ' % (a, v) - ret = ret[0:-2] + '}' - return ret - - def pack(self, mav, crc_extra, payload): - self._payload = payload - self._header = MAVLink_header(self._header.msgId, len(payload), mav.seq, - mav.srcSystem, mav.srcComponent) - self._msgbuf = self._header.pack() + payload - crc = mavutil.x25crc(self._msgbuf[1:]) - if False: # using CRC extra - crc.accumulate(chr(crc_extra)) - self._crc = crc.crc - self._msgbuf += struct.pack('hhhfiiffffff', self.mag_ofs_x, self.mag_ofs_y, self.mag_ofs_z, self.mag_declination, self.raw_press, self.raw_temp, self.gyro_cal_x, self.gyro_cal_y, self.gyro_cal_z, self.accel_cal_x, self.accel_cal_y, self.accel_cal_z)) - -class MAVLink_set_mag_offsets_message(MAVLink_message): - ''' - set the magnetometer offsets - ''' - def __init__(self, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_MAG_OFFSETS, 'SET_MAG_OFFSETS') - self._fieldnames = ['target_system', 'target_component', 'mag_ofs_x', 'mag_ofs_y', 'mag_ofs_z'] - self.target_system = target_system - self.target_component = target_component - self.mag_ofs_x = mag_ofs_x - self.mag_ofs_y = mag_ofs_y - self.mag_ofs_z = mag_ofs_z - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 29, struct.pack('>BBhhh', self.target_system, self.target_component, self.mag_ofs_x, self.mag_ofs_y, self.mag_ofs_z)) - -class MAVLink_meminfo_message(MAVLink_message): - ''' - state of APM memory - ''' - def __init__(self, brkval, freemem): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_MEMINFO, 'MEMINFO') - self._fieldnames = ['brkval', 'freemem'] - self.brkval = brkval - self.freemem = freemem - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 208, struct.pack('>HH', self.brkval, self.freemem)) - -class MAVLink_ap_adc_message(MAVLink_message): - ''' - raw ADC output - ''' - def __init__(self, adc1, adc2, adc3, adc4, adc5, adc6): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_AP_ADC, 'AP_ADC') - self._fieldnames = ['adc1', 'adc2', 'adc3', 'adc4', 'adc5', 'adc6'] - self.adc1 = adc1 - self.adc2 = adc2 - self.adc3 = adc3 - self.adc4 = adc4 - self.adc5 = adc5 - self.adc6 = adc6 - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 188, struct.pack('>HHHHHH', self.adc1, self.adc2, self.adc3, self.adc4, self.adc5, self.adc6)) - -class MAVLink_digicam_configure_message(MAVLink_message): - ''' - Configure on-board Camera Control System. - ''' - def __init__(self, target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, 'DIGICAM_CONFIGURE') - self._fieldnames = ['target_system', 'target_component', 'mode', 'shutter_speed', 'aperture', 'iso', 'exposure_type', 'command_id', 'engine_cut_off', 'extra_param', 'extra_value'] - self.target_system = target_system - self.target_component = target_component - self.mode = mode - self.shutter_speed = shutter_speed - self.aperture = aperture - self.iso = iso - self.exposure_type = exposure_type - self.command_id = command_id - self.engine_cut_off = engine_cut_off - self.extra_param = extra_param - self.extra_value = extra_value - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 118, struct.pack('>BBBHBBBBBBf', self.target_system, self.target_component, self.mode, self.shutter_speed, self.aperture, self.iso, self.exposure_type, self.command_id, self.engine_cut_off, self.extra_param, self.extra_value)) - -class MAVLink_digicam_control_message(MAVLink_message): - ''' - Control on-board Camera Control System to take shots. - ''' - def __init__(self, target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_DIGICAM_CONTROL, 'DIGICAM_CONTROL') - self._fieldnames = ['target_system', 'target_component', 'session', 'zoom_pos', 'zoom_step', 'focus_lock', 'shot', 'command_id', 'extra_param', 'extra_value'] - self.target_system = target_system - self.target_component = target_component - self.session = session - self.zoom_pos = zoom_pos - self.zoom_step = zoom_step - self.focus_lock = focus_lock - self.shot = shot - self.command_id = command_id - self.extra_param = extra_param - self.extra_value = extra_value - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 242, struct.pack('>BBBBbBBBBf', self.target_system, self.target_component, self.session, self.zoom_pos, self.zoom_step, self.focus_lock, self.shot, self.command_id, self.extra_param, self.extra_value)) - -class MAVLink_mount_configure_message(MAVLink_message): - ''' - Message to configure a camera mount, directional antenna, etc. - ''' - def __init__(self, target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_MOUNT_CONFIGURE, 'MOUNT_CONFIGURE') - self._fieldnames = ['target_system', 'target_component', 'mount_mode', 'stab_roll', 'stab_pitch', 'stab_yaw'] - self.target_system = target_system - self.target_component = target_component - self.mount_mode = mount_mode - self.stab_roll = stab_roll - self.stab_pitch = stab_pitch - self.stab_yaw = stab_yaw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 19, struct.pack('>BBBBBB', self.target_system, self.target_component, self.mount_mode, self.stab_roll, self.stab_pitch, self.stab_yaw)) - -class MAVLink_mount_control_message(MAVLink_message): - ''' - Message to control a camera mount, directional antenna, etc. - ''' - def __init__(self, target_system, target_component, input_a, input_b, input_c, save_position): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_MOUNT_CONTROL, 'MOUNT_CONTROL') - self._fieldnames = ['target_system', 'target_component', 'input_a', 'input_b', 'input_c', 'save_position'] - self.target_system = target_system - self.target_component = target_component - self.input_a = input_a - self.input_b = input_b - self.input_c = input_c - self.save_position = save_position - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 97, struct.pack('>BBiiiB', self.target_system, self.target_component, self.input_a, self.input_b, self.input_c, self.save_position)) - -class MAVLink_mount_status_message(MAVLink_message): - ''' - Message with some status from APM to GCS about camera or - antenna mount - ''' - def __init__(self, target_system, target_component, pointing_a, pointing_b, pointing_c): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_MOUNT_STATUS, 'MOUNT_STATUS') - self._fieldnames = ['target_system', 'target_component', 'pointing_a', 'pointing_b', 'pointing_c'] - self.target_system = target_system - self.target_component = target_component - self.pointing_a = pointing_a - self.pointing_b = pointing_b - self.pointing_c = pointing_c - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 233, struct.pack('>BBiii', self.target_system, self.target_component, self.pointing_a, self.pointing_b, self.pointing_c)) - -class MAVLink_fence_point_message(MAVLink_message): - ''' - A fence point. Used to set a point when from GCS - -> MAV. Also used to return a point from MAV -> GCS - ''' - def __init__(self, target_system, target_component, idx, count, lat, lng): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_POINT, 'FENCE_POINT') - self._fieldnames = ['target_system', 'target_component', 'idx', 'count', 'lat', 'lng'] - self.target_system = target_system - self.target_component = target_component - self.idx = idx - self.count = count - self.lat = lat - self.lng = lng - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 18, struct.pack('>BBBBff', self.target_system, self.target_component, self.idx, self.count, self.lat, self.lng)) - -class MAVLink_fence_fetch_point_message(MAVLink_message): - ''' - Request a current fence point from MAV - ''' - def __init__(self, target_system, target_component, idx): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_FETCH_POINT, 'FENCE_FETCH_POINT') - self._fieldnames = ['target_system', 'target_component', 'idx'] - self.target_system = target_system - self.target_component = target_component - self.idx = idx - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 68, struct.pack('>BBB', self.target_system, self.target_component, self.idx)) - -class MAVLink_fence_status_message(MAVLink_message): - ''' - Status of geo-fencing. Sent in extended status - stream when fencing enabled - ''' - def __init__(self, breach_status, breach_count, breach_type, breach_time): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_STATUS, 'FENCE_STATUS') - self._fieldnames = ['breach_status', 'breach_count', 'breach_type', 'breach_time'] - self.breach_status = breach_status - self.breach_count = breach_count - self.breach_type = breach_type - self.breach_time = breach_time - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 136, struct.pack('>BHBI', self.breach_status, self.breach_count, self.breach_type, self.breach_time)) - -class MAVLink_ahrs_message(MAVLink_message): - ''' - Status of DCM attitude estimator - ''' - def __init__(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_AHRS, 'AHRS') - self._fieldnames = ['omegaIx', 'omegaIy', 'omegaIz', 'accel_weight', 'renorm_val', 'error_rp', 'error_yaw'] - self.omegaIx = omegaIx - self.omegaIy = omegaIy - self.omegaIz = omegaIz - self.accel_weight = accel_weight - self.renorm_val = renorm_val - self.error_rp = error_rp - self.error_yaw = error_yaw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 127, struct.pack('>fffffff', self.omegaIx, self.omegaIy, self.omegaIz, self.accel_weight, self.renorm_val, self.error_rp, self.error_yaw)) - -class MAVLink_simstate_message(MAVLink_message): - ''' - Status of simulation environment, if used - ''' - def __init__(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SIMSTATE, 'SIMSTATE') - self._fieldnames = ['roll', 'pitch', 'yaw', 'xacc', 'yacc', 'zacc', 'xgyro', 'ygyro', 'zgyro'] - self.roll = roll - self.pitch = pitch - self.yaw = yaw - self.xacc = xacc - self.yacc = yacc - self.zacc = zacc - self.xgyro = xgyro - self.ygyro = ygyro - self.zgyro = zgyro - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 42, struct.pack('>fffffffff', self.roll, self.pitch, self.yaw, self.xacc, self.yacc, self.zacc, self.xgyro, self.ygyro, self.zgyro)) - -class MAVLink_hwstatus_message(MAVLink_message): - ''' - Status of key hardware - ''' - def __init__(self, Vcc, I2Cerr): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_HWSTATUS, 'HWSTATUS') - self._fieldnames = ['Vcc', 'I2Cerr'] - self.Vcc = Vcc - self.I2Cerr = I2Cerr - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 21, struct.pack('>HB', self.Vcc, self.I2Cerr)) - -class MAVLink_radio_message(MAVLink_message): - ''' - Status generated by radio - ''' - def __init__(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_RADIO, 'RADIO') - self._fieldnames = ['rssi', 'remrssi', 'txbuf', 'noise', 'remnoise', 'rxerrors', 'fixed'] - self.rssi = rssi - self.remrssi = remrssi - self.txbuf = txbuf - self.noise = noise - self.remnoise = remnoise - self.rxerrors = rxerrors - self.fixed = fixed - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 93, struct.pack('>BBBBBHH', self.rssi, self.remrssi, self.txbuf, self.noise, self.remnoise, self.rxerrors, self.fixed)) - -class MAVLink_heartbeat_message(MAVLink_message): - ''' - The heartbeat message shows that a system is present and - responding. The type of the MAV and Autopilot hardware allow - the receiving system to treat further messages from this - system appropriate (e.g. by laying out the user interface - based on the autopilot). - ''' - def __init__(self, type, autopilot, mavlink_version): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_HEARTBEAT, 'HEARTBEAT') - self._fieldnames = ['type', 'autopilot', 'mavlink_version'] - self.type = type - self.autopilot = autopilot - self.mavlink_version = mavlink_version - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 72, struct.pack('>BBB', self.type, self.autopilot, self.mavlink_version)) - -class MAVLink_boot_message(MAVLink_message): - ''' - The boot message indicates that a system is starting. The - onboard software version allows to keep track of onboard - soft/firmware revisions. - ''' - def __init__(self, version): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_BOOT, 'BOOT') - self._fieldnames = ['version'] - self.version = version - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 39, struct.pack('>I', self.version)) - -class MAVLink_system_time_message(MAVLink_message): - ''' - The system time is the time of the master clock, typically the - computer clock of the main onboard computer. - ''' - def __init__(self, time_usec): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SYSTEM_TIME, 'SYSTEM_TIME') - self._fieldnames = ['time_usec'] - self.time_usec = time_usec - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 190, struct.pack('>Q', self.time_usec)) - -class MAVLink_ping_message(MAVLink_message): - ''' - A ping message either requesting or responding to a ping. This - allows to measure the system latencies, including serial port, - radio modem and UDP connections. - ''' - def __init__(self, seq, target_system, target_component, time): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_PING, 'PING') - self._fieldnames = ['seq', 'target_system', 'target_component', 'time'] - self.seq = seq - self.target_system = target_system - self.target_component = target_component - self.time = time - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 92, struct.pack('>IBBQ', self.seq, self.target_system, self.target_component, self.time)) - -class MAVLink_system_time_utc_message(MAVLink_message): - ''' - UTC date and time from GPS module - ''' - def __init__(self, utc_date, utc_time): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SYSTEM_TIME_UTC, 'SYSTEM_TIME_UTC') - self._fieldnames = ['utc_date', 'utc_time'] - self.utc_date = utc_date - self.utc_time = utc_time - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 191, struct.pack('>II', self.utc_date, self.utc_time)) - -class MAVLink_change_operator_control_message(MAVLink_message): - ''' - Request to control this MAV - ''' - def __init__(self, target_system, control_request, version, passkey): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, 'CHANGE_OPERATOR_CONTROL') - self._fieldnames = ['target_system', 'control_request', 'version', 'passkey'] - self.target_system = target_system - self.control_request = control_request - self.version = version - self.passkey = passkey - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 217, struct.pack('>BBB25s', self.target_system, self.control_request, self.version, self.passkey)) - -class MAVLink_change_operator_control_ack_message(MAVLink_message): - ''' - Accept / deny control of this MAV - ''' - def __init__(self, gcs_system_id, control_request, ack): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, 'CHANGE_OPERATOR_CONTROL_ACK') - self._fieldnames = ['gcs_system_id', 'control_request', 'ack'] - self.gcs_system_id = gcs_system_id - self.control_request = control_request - self.ack = ack - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 104, struct.pack('>BBB', self.gcs_system_id, self.control_request, self.ack)) - -class MAVLink_auth_key_message(MAVLink_message): - ''' - Emit an encrypted signature / key identifying this system. - PLEASE NOTE: This protocol has been kept simple, so - transmitting the key requires an encrypted channel for true - safety. - ''' - def __init__(self, key): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_AUTH_KEY, 'AUTH_KEY') - self._fieldnames = ['key'] - self.key = key - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 119, struct.pack('>32s', self.key)) - -class MAVLink_action_ack_message(MAVLink_message): - ''' - This message acknowledges an action. IMPORTANT: The - acknowledgement can be also negative, e.g. the MAV rejects a - reset message because it is in-flight. The action ids are - defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h - ''' - def __init__(self, action, result): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_ACTION_ACK, 'ACTION_ACK') - self._fieldnames = ['action', 'result'] - self.action = action - self.result = result - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 219, struct.pack('>BB', self.action, self.result)) - -class MAVLink_action_message(MAVLink_message): - ''' - An action message allows to execute a certain onboard action. - These include liftoff, land, storing parameters too EEPROM, - shutddown, etc. The action ids are defined in ENUM MAV_ACTION - in mavlink/include/mavlink_types.h - ''' - def __init__(self, target, target_component, action): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_ACTION, 'ACTION') - self._fieldnames = ['target', 'target_component', 'action'] - self.target = target - self.target_component = target_component - self.action = action - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 60, struct.pack('>BBB', self.target, self.target_component, self.action)) - -class MAVLink_set_mode_message(MAVLink_message): - ''' - Set the system mode, as defined by enum MAV_MODE in - mavlink/include/mavlink_types.h. There is no target component - id as the mode is by definition for the overall aircraft, not - only for one component. - ''' - def __init__(self, target, mode): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_MODE, 'SET_MODE') - self._fieldnames = ['target', 'mode'] - self.target = target - self.mode = mode - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 186, struct.pack('>BB', self.target, self.mode)) - -class MAVLink_set_nav_mode_message(MAVLink_message): - ''' - Set the system navigation mode, as defined by enum - MAV_NAV_MODE in mavlink/include/mavlink_types.h. The - navigation mode applies to the whole aircraft and thus all - components. - ''' - def __init__(self, target, nav_mode): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_NAV_MODE, 'SET_NAV_MODE') - self._fieldnames = ['target', 'nav_mode'] - self.target = target - self.nav_mode = nav_mode - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 10, struct.pack('>BB', self.target, self.nav_mode)) - -class MAVLink_param_request_read_message(MAVLink_message): - ''' - Request to read the onboard parameter with the param_id string - id. Onboard parameters are stored as key[const char*] -> - value[float]. This allows to send a parameter to any other - component (such as the GCS) without the need of previous - knowledge of possible parameter names. Thus the same GCS can - store different parameters for different autopilots. See also - http://qgroundcontrol.org/parameter_interface for a full - documentation of QGroundControl and IMU code. - ''' - def __init__(self, target_system, target_component, param_id, param_index): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_REQUEST_READ, 'PARAM_REQUEST_READ') - self._fieldnames = ['target_system', 'target_component', 'param_id', 'param_index'] - self.target_system = target_system - self.target_component = target_component - self.param_id = param_id - self.param_index = param_index - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 89, struct.pack('>BB15sh', self.target_system, self.target_component, self.param_id, self.param_index)) - -class MAVLink_param_request_list_message(MAVLink_message): - ''' - Request all parameters of this component. After his request, - all parameters are emitted. - ''' - def __init__(self, target_system, target_component): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, 'PARAM_REQUEST_LIST') - self._fieldnames = ['target_system', 'target_component'] - self.target_system = target_system - self.target_component = target_component - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 159, struct.pack('>BB', self.target_system, self.target_component)) - -class MAVLink_param_value_message(MAVLink_message): - ''' - Emit the value of a onboard parameter. The inclusion of - param_count and param_index in the message allows the - recipient to keep track of received parameters and allows him - to re-request missing parameters after a loss or timeout. - ''' - def __init__(self, param_id, param_value, param_count, param_index): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_VALUE, 'PARAM_VALUE') - self._fieldnames = ['param_id', 'param_value', 'param_count', 'param_index'] - self.param_id = param_id - self.param_value = param_value - self.param_count = param_count - self.param_index = param_index - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 162, struct.pack('>15sfHH', self.param_id, self.param_value, self.param_count, self.param_index)) - -class MAVLink_param_set_message(MAVLink_message): - ''' - Set a parameter value TEMPORARILY to RAM. It will be reset to - default on system reboot. Send the ACTION - MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents - to EEPROM. IMPORTANT: The receiving component should - acknowledge the new parameter value by sending a param_value - message to all communication partners. This will also ensure - that multiple GCS all have an up-to-date list of all - parameters. If the sending GCS did not receive a PARAM_VALUE - message within its timeout time, it should re-send the - PARAM_SET message. - ''' - def __init__(self, target_system, target_component, param_id, param_value): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_SET, 'PARAM_SET') - self._fieldnames = ['target_system', 'target_component', 'param_id', 'param_value'] - self.target_system = target_system - self.target_component = target_component - self.param_id = param_id - self.param_value = param_value - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 121, struct.pack('>BB15sf', self.target_system, self.target_component, self.param_id, self.param_value)) - -class MAVLink_gps_raw_int_message(MAVLink_message): - ''' - The global position, as returned by the Global Positioning - System (GPS). This is NOT the global position estimate of the - sytem, but rather a RAW sensor value. See message - GLOBAL_POSITION for the global position estimate. Coordinate - frame is right-handed, Z-axis up (GPS frame) - ''' - def __init__(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_RAW_INT, 'GPS_RAW_INT') - self._fieldnames = ['usec', 'fix_type', 'lat', 'lon', 'alt', 'eph', 'epv', 'v', 'hdg'] - self.usec = usec - self.fix_type = fix_type - self.lat = lat - self.lon = lon - self.alt = alt - self.eph = eph - self.epv = epv - self.v = v - self.hdg = hdg - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 149, struct.pack('>QBiiiffff', self.usec, self.fix_type, self.lat, self.lon, self.alt, self.eph, self.epv, self.v, self.hdg)) - -class MAVLink_scaled_imu_message(MAVLink_message): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This - message should contain the scaled values to the described - units - ''' - def __init__(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SCALED_IMU, 'SCALED_IMU') - self._fieldnames = ['usec', 'xacc', 'yacc', 'zacc', 'xgyro', 'ygyro', 'zgyro', 'xmag', 'ymag', 'zmag'] - self.usec = usec - self.xacc = xacc - self.yacc = yacc - self.zacc = zacc - self.xgyro = xgyro - self.ygyro = ygyro - self.zgyro = zgyro - self.xmag = xmag - self.ymag = ymag - self.zmag = zmag - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 222, struct.pack('>Qhhhhhhhhh', self.usec, self.xacc, self.yacc, self.zacc, self.xgyro, self.ygyro, self.zgyro, self.xmag, self.ymag, self.zmag)) - -class MAVLink_gps_status_message(MAVLink_message): - ''' - The positioning status, as reported by GPS. This message is - intended to display status information about each satellite - visible to the receiver. See message GLOBAL_POSITION for the - global position estimate. This message can contain information - for up to 20 satellites. - ''' - def __init__(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_STATUS, 'GPS_STATUS') - self._fieldnames = ['satellites_visible', 'satellite_prn', 'satellite_used', 'satellite_elevation', 'satellite_azimuth', 'satellite_snr'] - self.satellites_visible = satellites_visible - self.satellite_prn = satellite_prn - self.satellite_used = satellite_used - self.satellite_elevation = satellite_elevation - self.satellite_azimuth = satellite_azimuth - self.satellite_snr = satellite_snr - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 110, struct.pack('>B20s20s20s20s20s', self.satellites_visible, self.satellite_prn, self.satellite_used, self.satellite_elevation, self.satellite_azimuth, self.satellite_snr)) - -class MAVLink_raw_imu_message(MAVLink_message): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This - message should always contain the true raw values without any - scaling to allow data capture and system debugging. - ''' - def __init__(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_RAW_IMU, 'RAW_IMU') - self._fieldnames = ['usec', 'xacc', 'yacc', 'zacc', 'xgyro', 'ygyro', 'zgyro', 'xmag', 'ymag', 'zmag'] - self.usec = usec - self.xacc = xacc - self.yacc = yacc - self.zacc = zacc - self.xgyro = xgyro - self.ygyro = ygyro - self.zgyro = zgyro - self.xmag = xmag - self.ymag = ymag - self.zmag = zmag - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 179, struct.pack('>Qhhhhhhhhh', self.usec, self.xacc, self.yacc, self.zacc, self.xgyro, self.ygyro, self.zgyro, self.xmag, self.ymag, self.zmag)) - -class MAVLink_raw_pressure_message(MAVLink_message): - ''' - The RAW pressure readings for the typical setup of one - absolute pressure and one differential pressure sensor. The - sensor values should be the raw, UNSCALED ADC values. - ''' - def __init__(self, usec, press_abs, press_diff1, press_diff2, temperature): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_RAW_PRESSURE, 'RAW_PRESSURE') - self._fieldnames = ['usec', 'press_abs', 'press_diff1', 'press_diff2', 'temperature'] - self.usec = usec - self.press_abs = press_abs - self.press_diff1 = press_diff1 - self.press_diff2 = press_diff2 - self.temperature = temperature - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 136, struct.pack('>Qhhhh', self.usec, self.press_abs, self.press_diff1, self.press_diff2, self.temperature)) - -class MAVLink_scaled_pressure_message(MAVLink_message): - ''' - The pressure readings for the typical setup of one absolute - and differential pressure sensor. The units are as specified - in each field. - ''' - def __init__(self, usec, press_abs, press_diff, temperature): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SCALED_PRESSURE, 'SCALED_PRESSURE') - self._fieldnames = ['usec', 'press_abs', 'press_diff', 'temperature'] - self.usec = usec - self.press_abs = press_abs - self.press_diff = press_diff - self.temperature = temperature - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 229, struct.pack('>Qffh', self.usec, self.press_abs, self.press_diff, self.temperature)) - -class MAVLink_attitude_message(MAVLink_message): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, - X-front, Y-right). - ''' - def __init__(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_ATTITUDE, 'ATTITUDE') - self._fieldnames = ['usec', 'roll', 'pitch', 'yaw', 'rollspeed', 'pitchspeed', 'yawspeed'] - self.usec = usec - self.roll = roll - self.pitch = pitch - self.yaw = yaw - self.rollspeed = rollspeed - self.pitchspeed = pitchspeed - self.yawspeed = yawspeed - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 66, struct.pack('>Qffffff', self.usec, self.roll, self.pitch, self.yaw, self.rollspeed, self.pitchspeed, self.yawspeed)) - -class MAVLink_local_position_message(MAVLink_message): - ''' - The filtered local position (e.g. fused computer vision and - accelerometers). Coordinate frame is right-handed, Z-axis down - (aeronautical frame) - ''' - def __init__(self, usec, x, y, z, vx, vy, vz): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_LOCAL_POSITION, 'LOCAL_POSITION') - self._fieldnames = ['usec', 'x', 'y', 'z', 'vx', 'vy', 'vz'] - self.usec = usec - self.x = x - self.y = y - self.z = z - self.vx = vx - self.vy = vy - self.vz = vz - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 126, struct.pack('>Qffffff', self.usec, self.x, self.y, self.z, self.vx, self.vy, self.vz)) - -class MAVLink_global_position_message(MAVLink_message): - ''' - The filtered global position (e.g. fused GPS and - accelerometers). Coordinate frame is right-handed, Z-axis up - (GPS frame) - ''' - def __init__(self, usec, lat, lon, alt, vx, vy, vz): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GLOBAL_POSITION, 'GLOBAL_POSITION') - self._fieldnames = ['usec', 'lat', 'lon', 'alt', 'vx', 'vy', 'vz'] - self.usec = usec - self.lat = lat - self.lon = lon - self.alt = alt - self.vx = vx - self.vy = vy - self.vz = vz - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 147, struct.pack('>Qffffff', self.usec, self.lat, self.lon, self.alt, self.vx, self.vy, self.vz)) - -class MAVLink_gps_raw_message(MAVLink_message): - ''' - The global position, as returned by the Global Positioning - System (GPS). This is NOT the global position estimate of the - sytem, but rather a RAW sensor value. See message - GLOBAL_POSITION for the global position estimate. Coordinate - frame is right-handed, Z-axis up (GPS frame) - ''' - def __init__(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_RAW, 'GPS_RAW') - self._fieldnames = ['usec', 'fix_type', 'lat', 'lon', 'alt', 'eph', 'epv', 'v', 'hdg'] - self.usec = usec - self.fix_type = fix_type - self.lat = lat - self.lon = lon - self.alt = alt - self.eph = eph - self.epv = epv - self.v = v - self.hdg = hdg - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 185, struct.pack('>QBfffffff', self.usec, self.fix_type, self.lat, self.lon, self.alt, self.eph, self.epv, self.v, self.hdg)) - -class MAVLink_sys_status_message(MAVLink_message): - ''' - The general system state. If the system is following the - MAVLink standard, the system state is mainly defined by three - orthogonal states/modes: The system mode, which is either - LOCKED (motors shut down and locked), MANUAL (system under RC - control), GUIDED (system with autonomous position control, - position setpoint controlled manually) or AUTO (system guided - by path/waypoint planner). The NAV_MODE defined the current - flight state: LIFTOFF (often an open-loop maneuver), LANDING, - WAYPOINTS or VECTOR. This represents the internal navigation - state machine. The system status shows wether the system is - currently active or not and if an emergency occured. During - the CRITICAL and EMERGENCY states the MAV is still considered - to be active, but should start emergency procedures - autonomously. After a failure occured it should first move - from active to critical to allow manual intervention and then - move to emergency after a certain timeout. - ''' - def __init__(self, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SYS_STATUS, 'SYS_STATUS') - self._fieldnames = ['mode', 'nav_mode', 'status', 'load', 'vbat', 'battery_remaining', 'packet_drop'] - self.mode = mode - self.nav_mode = nav_mode - self.status = status - self.load = load - self.vbat = vbat - self.battery_remaining = battery_remaining - self.packet_drop = packet_drop - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 112, struct.pack('>BBBHHHH', self.mode, self.nav_mode, self.status, self.load, self.vbat, self.battery_remaining, self.packet_drop)) - -class MAVLink_rc_channels_raw_message(MAVLink_message): - ''' - The RAW values of the RC channels received. The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. Individual receivers/transmitters might - violate this specification. - ''' - def __init__(self, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_RC_CHANNELS_RAW, 'RC_CHANNELS_RAW') - self._fieldnames = ['chan1_raw', 'chan2_raw', 'chan3_raw', 'chan4_raw', 'chan5_raw', 'chan6_raw', 'chan7_raw', 'chan8_raw', 'rssi'] - self.chan1_raw = chan1_raw - self.chan2_raw = chan2_raw - self.chan3_raw = chan3_raw - self.chan4_raw = chan4_raw - self.chan5_raw = chan5_raw - self.chan6_raw = chan6_raw - self.chan7_raw = chan7_raw - self.chan8_raw = chan8_raw - self.rssi = rssi - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 252, struct.pack('>HHHHHHHHB', self.chan1_raw, self.chan2_raw, self.chan3_raw, self.chan4_raw, self.chan5_raw, self.chan6_raw, self.chan7_raw, self.chan8_raw, self.rssi)) - -class MAVLink_rc_channels_scaled_message(MAVLink_message): - ''' - The scaled values of the RC channels received. (-100%) -10000, - (0%) 0, (100%) 10000 - ''' - def __init__(self, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, 'RC_CHANNELS_SCALED') - self._fieldnames = ['chan1_scaled', 'chan2_scaled', 'chan3_scaled', 'chan4_scaled', 'chan5_scaled', 'chan6_scaled', 'chan7_scaled', 'chan8_scaled', 'rssi'] - self.chan1_scaled = chan1_scaled - self.chan2_scaled = chan2_scaled - self.chan3_scaled = chan3_scaled - self.chan4_scaled = chan4_scaled - self.chan5_scaled = chan5_scaled - self.chan6_scaled = chan6_scaled - self.chan7_scaled = chan7_scaled - self.chan8_scaled = chan8_scaled - self.rssi = rssi - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 162, struct.pack('>hhhhhhhhB', self.chan1_scaled, self.chan2_scaled, self.chan3_scaled, self.chan4_scaled, self.chan5_scaled, self.chan6_scaled, self.chan7_scaled, self.chan8_scaled, self.rssi)) - -class MAVLink_servo_output_raw_message(MAVLink_message): - ''' - The RAW values of the servo outputs (for RC input from the - remote, use the RC_CHANNELS messages). The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. - ''' - def __init__(self, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 'SERVO_OUTPUT_RAW') - self._fieldnames = ['servo1_raw', 'servo2_raw', 'servo3_raw', 'servo4_raw', 'servo5_raw', 'servo6_raw', 'servo7_raw', 'servo8_raw'] - self.servo1_raw = servo1_raw - self.servo2_raw = servo2_raw - self.servo3_raw = servo3_raw - self.servo4_raw = servo4_raw - self.servo5_raw = servo5_raw - self.servo6_raw = servo6_raw - self.servo7_raw = servo7_raw - self.servo8_raw = servo8_raw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 215, struct.pack('>HHHHHHHH', self.servo1_raw, self.servo2_raw, self.servo3_raw, self.servo4_raw, self.servo5_raw, self.servo6_raw, self.servo7_raw, self.servo8_raw)) - -class MAVLink_waypoint_message(MAVLink_message): - ''' - Message encoding a waypoint. This message is emitted to - announce the presence of a waypoint and to set a waypoint - on the system. The waypoint can be either in x, y, z meters - (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is - Z-down, right handed, global frame is Z-up, right handed - ''' - def __init__(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT, 'WAYPOINT') - self._fieldnames = ['target_system', 'target_component', 'seq', 'frame', 'command', 'current', 'autocontinue', 'param1', 'param2', 'param3', 'param4', 'x', 'y', 'z'] - self.target_system = target_system - self.target_component = target_component - self.seq = seq - self.frame = frame - self.command = command - self.current = current - self.autocontinue = autocontinue - self.param1 = param1 - self.param2 = param2 - self.param3 = param3 - self.param4 = param4 - self.x = x - self.y = y - self.z = z - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 128, struct.pack('>BBHBBBBfffffff', self.target_system, self.target_component, self.seq, self.frame, self.command, self.current, self.autocontinue, self.param1, self.param2, self.param3, self.param4, self.x, self.y, self.z)) - -class MAVLink_waypoint_request_message(MAVLink_message): - ''' - Request the information of the waypoint with the sequence - number seq. The response of the system to this message should - be a WAYPOINT message. - ''' - def __init__(self, target_system, target_component, seq): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_REQUEST, 'WAYPOINT_REQUEST') - self._fieldnames = ['target_system', 'target_component', 'seq'] - self.target_system = target_system - self.target_component = target_component - self.seq = seq - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 9, struct.pack('>BBH', self.target_system, self.target_component, self.seq)) - -class MAVLink_waypoint_set_current_message(MAVLink_message): - ''' - Set the waypoint with sequence number seq as current waypoint. - This means that the MAV will continue to this waypoint on the - shortest path (not following the waypoints in-between). - ''' - def __init__(self, target_system, target_component, seq): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, 'WAYPOINT_SET_CURRENT') - self._fieldnames = ['target_system', 'target_component', 'seq'] - self.target_system = target_system - self.target_component = target_component - self.seq = seq - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 106, struct.pack('>BBH', self.target_system, self.target_component, self.seq)) - -class MAVLink_waypoint_current_message(MAVLink_message): - ''' - Message that announces the sequence number of the current - active waypoint. The MAV will fly towards this waypoint. - ''' - def __init__(self, seq): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_CURRENT, 'WAYPOINT_CURRENT') - self._fieldnames = ['seq'] - self.seq = seq - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 101, struct.pack('>H', self.seq)) - -class MAVLink_waypoint_request_list_message(MAVLink_message): - ''' - Request the overall list of waypoints from the - system/component. - ''' - def __init__(self, target_system, target_component): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, 'WAYPOINT_REQUEST_LIST') - self._fieldnames = ['target_system', 'target_component'] - self.target_system = target_system - self.target_component = target_component - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 213, struct.pack('>BB', self.target_system, self.target_component)) - -class MAVLink_waypoint_count_message(MAVLink_message): - ''' - This message is emitted as response to WAYPOINT_REQUEST_LIST - by the MAV. The GCS can then request the individual waypoints - based on the knowledge of the total number of waypoints. - ''' - def __init__(self, target_system, target_component, count): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_COUNT, 'WAYPOINT_COUNT') - self._fieldnames = ['target_system', 'target_component', 'count'] - self.target_system = target_system - self.target_component = target_component - self.count = count - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 4, struct.pack('>BBH', self.target_system, self.target_component, self.count)) - -class MAVLink_waypoint_clear_all_message(MAVLink_message): - ''' - Delete all waypoints at once. - ''' - def __init__(self, target_system, target_component): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL, 'WAYPOINT_CLEAR_ALL') - self._fieldnames = ['target_system', 'target_component'] - self.target_system = target_system - self.target_component = target_component - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 229, struct.pack('>BB', self.target_system, self.target_component)) - -class MAVLink_waypoint_reached_message(MAVLink_message): - ''' - A certain waypoint has been reached. The system will either - hold this position (or circle on the orbit) or (if the - autocontinue on the WP was set) continue to the next waypoint. - ''' - def __init__(self, seq): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_REACHED, 'WAYPOINT_REACHED') - self._fieldnames = ['seq'] - self.seq = seq - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 21, struct.pack('>H', self.seq)) - -class MAVLink_waypoint_ack_message(MAVLink_message): - ''' - Ack message during waypoint handling. The type field states if - this message is a positive ack (type=0) or if an error - happened (type=non-zero). - ''' - def __init__(self, target_system, target_component, type): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_ACK, 'WAYPOINT_ACK') - self._fieldnames = ['target_system', 'target_component', 'type'] - self.target_system = target_system - self.target_component = target_component - self.type = type - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 214, struct.pack('>BBB', self.target_system, self.target_component, self.type)) - -class MAVLink_gps_set_global_origin_message(MAVLink_message): - ''' - As local waypoints exist, the global waypoint reference allows - to transform between the local coordinate frame and the global - (GPS) coordinate frame. This can be necessary when e.g. in- - and outdoor settings are connected and the MAV should move - from in- to outdoor. - ''' - def __init__(self, target_system, target_component, latitude, longitude, altitude): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN, 'GPS_SET_GLOBAL_ORIGIN') - self._fieldnames = ['target_system', 'target_component', 'latitude', 'longitude', 'altitude'] - self.target_system = target_system - self.target_component = target_component - self.latitude = latitude - self.longitude = longitude - self.altitude = altitude - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 215, struct.pack('>BBiii', self.target_system, self.target_component, self.latitude, self.longitude, self.altitude)) - -class MAVLink_gps_local_origin_set_message(MAVLink_message): - ''' - Once the MAV sets a new GPS-Local correspondence, this message - announces the origin (0,0,0) position - ''' - def __init__(self, latitude, longitude, altitude): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET, 'GPS_LOCAL_ORIGIN_SET') - self._fieldnames = ['latitude', 'longitude', 'altitude'] - self.latitude = latitude - self.longitude = longitude - self.altitude = altitude - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 14, struct.pack('>iii', self.latitude, self.longitude, self.altitude)) - -class MAVLink_local_position_setpoint_set_message(MAVLink_message): - ''' - Set the setpoint for a local position controller. This is the - position in local coordinates the MAV should fly to. This - message is sent by the path/waypoint planner to the onboard - position controller. As some MAVs have a degree of freedom in - yaw (e.g. all helicopters/quadrotors), the desired yaw angle - is part of the message. - ''' - def __init__(self, target_system, target_component, x, y, z, yaw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET, 'LOCAL_POSITION_SETPOINT_SET') - self._fieldnames = ['target_system', 'target_component', 'x', 'y', 'z', 'yaw'] - self.target_system = target_system - self.target_component = target_component - self.x = x - self.y = y - self.z = z - self.yaw = yaw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 206, struct.pack('>BBffff', self.target_system, self.target_component, self.x, self.y, self.z, self.yaw)) - -class MAVLink_local_position_setpoint_message(MAVLink_message): - ''' - Transmit the current local setpoint of the controller to other - MAVs (collision avoidance) and to the GCS. - ''' - def __init__(self, x, y, z, yaw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, 'LOCAL_POSITION_SETPOINT') - self._fieldnames = ['x', 'y', 'z', 'yaw'] - self.x = x - self.y = y - self.z = z - self.yaw = yaw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 50, struct.pack('>ffff', self.x, self.y, self.z, self.yaw)) - -class MAVLink_control_status_message(MAVLink_message): - ''' - - ''' - def __init__(self, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_CONTROL_STATUS, 'CONTROL_STATUS') - self._fieldnames = ['position_fix', 'vision_fix', 'gps_fix', 'ahrs_health', 'control_att', 'control_pos_xy', 'control_pos_z', 'control_pos_yaw'] - self.position_fix = position_fix - self.vision_fix = vision_fix - self.gps_fix = gps_fix - self.ahrs_health = ahrs_health - self.control_att = control_att - self.control_pos_xy = control_pos_xy - self.control_pos_z = control_pos_z - self.control_pos_yaw = control_pos_yaw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 157, struct.pack('>BBBBBBBB', self.position_fix, self.vision_fix, self.gps_fix, self.ahrs_health, self.control_att, self.control_pos_xy, self.control_pos_z, self.control_pos_yaw)) - -class MAVLink_safety_set_allowed_area_message(MAVLink_message): - ''' - Set a safety zone (volume), which is defined by two corners of - a cube. This message can be used to tell the MAV which - setpoints/waypoints to accept and which to reject. Safety - areas are often enforced by national or competition - regulations. - ''' - def __init__(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, 'SAFETY_SET_ALLOWED_AREA') - self._fieldnames = ['target_system', 'target_component', 'frame', 'p1x', 'p1y', 'p1z', 'p2x', 'p2y', 'p2z'] - self.target_system = target_system - self.target_component = target_component - self.frame = frame - self.p1x = p1x - self.p1y = p1y - self.p1z = p1z - self.p2x = p2x - self.p2y = p2y - self.p2z = p2z - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 126, struct.pack('>BBBffffff', self.target_system, self.target_component, self.frame, self.p1x, self.p1y, self.p1z, self.p2x, self.p2y, self.p2z)) - -class MAVLink_safety_allowed_area_message(MAVLink_message): - ''' - Read out the safety zone the MAV currently assumes. - ''' - def __init__(self, frame, p1x, p1y, p1z, p2x, p2y, p2z): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, 'SAFETY_ALLOWED_AREA') - self._fieldnames = ['frame', 'p1x', 'p1y', 'p1z', 'p2x', 'p2y', 'p2z'] - self.frame = frame - self.p1x = p1x - self.p1y = p1y - self.p1z = p1z - self.p2x = p2x - self.p2y = p2y - self.p2z = p2z - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 108, struct.pack('>Bffffff', self.frame, self.p1x, self.p1y, self.p1z, self.p2x, self.p2y, self.p2z)) - -class MAVLink_set_roll_pitch_yaw_thrust_message(MAVLink_message): - ''' - Set roll, pitch and yaw. - ''' - def __init__(self, target_system, target_component, roll, pitch, yaw, thrust): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, 'SET_ROLL_PITCH_YAW_THRUST') - self._fieldnames = ['target_system', 'target_component', 'roll', 'pitch', 'yaw', 'thrust'] - self.target_system = target_system - self.target_component = target_component - self.roll = roll - self.pitch = pitch - self.yaw = yaw - self.thrust = thrust - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 213, struct.pack('>BBffff', self.target_system, self.target_component, self.roll, self.pitch, self.yaw, self.thrust)) - -class MAVLink_set_roll_pitch_yaw_speed_thrust_message(MAVLink_message): - ''' - Set roll, pitch and yaw. - ''' - def __init__(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, 'SET_ROLL_PITCH_YAW_SPEED_THRUST') - self._fieldnames = ['target_system', 'target_component', 'roll_speed', 'pitch_speed', 'yaw_speed', 'thrust'] - self.target_system = target_system - self.target_component = target_component - self.roll_speed = roll_speed - self.pitch_speed = pitch_speed - self.yaw_speed = yaw_speed - self.thrust = thrust - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 95, struct.pack('>BBffff', self.target_system, self.target_component, self.roll_speed, self.pitch_speed, self.yaw_speed, self.thrust)) - -class MAVLink_roll_pitch_yaw_thrust_setpoint_message(MAVLink_message): - ''' - Setpoint in roll, pitch, yaw currently active on the system. - ''' - def __init__(self, time_us, roll, pitch, yaw, thrust): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, 'ROLL_PITCH_YAW_THRUST_SETPOINT') - self._fieldnames = ['time_us', 'roll', 'pitch', 'yaw', 'thrust'] - self.time_us = time_us - self.roll = roll - self.pitch = pitch - self.yaw = yaw - self.thrust = thrust - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 5, struct.pack('>Qffff', self.time_us, self.roll, self.pitch, self.yaw, self.thrust)) - -class MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message(MAVLink_message): - ''' - Setpoint in rollspeed, pitchspeed, yawspeed currently active - on the system. - ''' - def __init__(self, time_us, roll_speed, pitch_speed, yaw_speed, thrust): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, 'ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT') - self._fieldnames = ['time_us', 'roll_speed', 'pitch_speed', 'yaw_speed', 'thrust'] - self.time_us = time_us - self.roll_speed = roll_speed - self.pitch_speed = pitch_speed - self.yaw_speed = yaw_speed - self.thrust = thrust - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 127, struct.pack('>Qffff', self.time_us, self.roll_speed, self.pitch_speed, self.yaw_speed, self.thrust)) - -class MAVLink_nav_controller_output_message(MAVLink_message): - ''' - Outputs of the APM navigation controller. The primary use of - this message is to check the response and signs of the - controller before actual flight and to assist with tuning - controller parameters - ''' - def __init__(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, 'NAV_CONTROLLER_OUTPUT') - self._fieldnames = ['nav_roll', 'nav_pitch', 'nav_bearing', 'target_bearing', 'wp_dist', 'alt_error', 'aspd_error', 'xtrack_error'] - self.nav_roll = nav_roll - self.nav_pitch = nav_pitch - self.nav_bearing = nav_bearing - self.target_bearing = target_bearing - self.wp_dist = wp_dist - self.alt_error = alt_error - self.aspd_error = aspd_error - self.xtrack_error = xtrack_error - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 57, struct.pack('>ffhhHfff', self.nav_roll, self.nav_pitch, self.nav_bearing, self.target_bearing, self.wp_dist, self.alt_error, self.aspd_error, self.xtrack_error)) - -class MAVLink_position_target_message(MAVLink_message): - ''' - The goal position of the system. This position is the input to - any navigation or path planning algorithm and does NOT - represent the current controller setpoint. - ''' - def __init__(self, x, y, z, yaw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_POSITION_TARGET, 'POSITION_TARGET') - self._fieldnames = ['x', 'y', 'z', 'yaw'] - self.x = x - self.y = y - self.z = z - self.yaw = yaw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 126, struct.pack('>ffff', self.x, self.y, self.z, self.yaw)) - -class MAVLink_state_correction_message(MAVLink_message): - ''' - Corrects the systems state by adding an error correction term - to the position and velocity, and by rotating the attitude by - a correction angle. - ''' - def __init__(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_STATE_CORRECTION, 'STATE_CORRECTION') - self._fieldnames = ['xErr', 'yErr', 'zErr', 'rollErr', 'pitchErr', 'yawErr', 'vxErr', 'vyErr', 'vzErr'] - self.xErr = xErr - self.yErr = yErr - self.zErr = zErr - self.rollErr = rollErr - self.pitchErr = pitchErr - self.yawErr = yawErr - self.vxErr = vxErr - self.vyErr = vyErr - self.vzErr = vzErr - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 130, struct.pack('>fffffffff', self.xErr, self.yErr, self.zErr, self.rollErr, self.pitchErr, self.yawErr, self.vxErr, self.vyErr, self.vzErr)) - -class MAVLink_set_altitude_message(MAVLink_message): - ''' - - ''' - def __init__(self, target, mode): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_ALTITUDE, 'SET_ALTITUDE') - self._fieldnames = ['target', 'mode'] - self.target = target - self.mode = mode - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 119, struct.pack('>BI', self.target, self.mode)) - -class MAVLink_request_data_stream_message(MAVLink_message): - ''' - - ''' - def __init__(self, target_system, target_component, req_stream_id, req_message_rate, start_stop): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, 'REQUEST_DATA_STREAM') - self._fieldnames = ['target_system', 'target_component', 'req_stream_id', 'req_message_rate', 'start_stop'] - self.target_system = target_system - self.target_component = target_component - self.req_stream_id = req_stream_id - self.req_message_rate = req_message_rate - self.start_stop = start_stop - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 193, struct.pack('>BBBHB', self.target_system, self.target_component, self.req_stream_id, self.req_message_rate, self.start_stop)) - -class MAVLink_hil_state_message(MAVLink_message): - ''' - This packet is useful for high throughput - applications such as hardware in the loop simulations. - ''' - def __init__(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_HIL_STATE, 'HIL_STATE') - self._fieldnames = ['usec', 'roll', 'pitch', 'yaw', 'rollspeed', 'pitchspeed', 'yawspeed', 'lat', 'lon', 'alt', 'vx', 'vy', 'vz', 'xacc', 'yacc', 'zacc'] - self.usec = usec - self.roll = roll - self.pitch = pitch - self.yaw = yaw - self.rollspeed = rollspeed - self.pitchspeed = pitchspeed - self.yawspeed = yawspeed - self.lat = lat - self.lon = lon - self.alt = alt - self.vx = vx - self.vy = vy - self.vz = vz - self.xacc = xacc - self.yacc = yacc - self.zacc = zacc - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 191, struct.pack('>Qffffffiiihhhhhh', self.usec, self.roll, self.pitch, self.yaw, self.rollspeed, self.pitchspeed, self.yawspeed, self.lat, self.lon, self.alt, self.vx, self.vy, self.vz, self.xacc, self.yacc, self.zacc)) - -class MAVLink_hil_controls_message(MAVLink_message): - ''' - Hardware in the loop control outputs - ''' - def __init__(self, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_HIL_CONTROLS, 'HIL_CONTROLS') - self._fieldnames = ['time_us', 'roll_ailerons', 'pitch_elevator', 'yaw_rudder', 'throttle', 'mode', 'nav_mode'] - self.time_us = time_us - self.roll_ailerons = roll_ailerons - self.pitch_elevator = pitch_elevator - self.yaw_rudder = yaw_rudder - self.throttle = throttle - self.mode = mode - self.nav_mode = nav_mode - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 236, struct.pack('>QffffBB', self.time_us, self.roll_ailerons, self.pitch_elevator, self.yaw_rudder, self.throttle, self.mode, self.nav_mode)) - -class MAVLink_manual_control_message(MAVLink_message): - ''' - - ''' - def __init__(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_MANUAL_CONTROL, 'MANUAL_CONTROL') - self._fieldnames = ['target', 'roll', 'pitch', 'yaw', 'thrust', 'roll_manual', 'pitch_manual', 'yaw_manual', 'thrust_manual'] - self.target = target - self.roll = roll - self.pitch = pitch - self.yaw = yaw - self.thrust = thrust - self.roll_manual = roll_manual - self.pitch_manual = pitch_manual - self.yaw_manual = yaw_manual - self.thrust_manual = thrust_manual - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 158, struct.pack('>BffffBBBB', self.target, self.roll, self.pitch, self.yaw, self.thrust, self.roll_manual, self.pitch_manual, self.yaw_manual, self.thrust_manual)) - -class MAVLink_rc_channels_override_message(MAVLink_message): - ''' - The RAW values of the RC channels sent to the MAV to override - info received from the RC radio. A value of -1 means no change - to that channel. A value of 0 means control of that channel - should be released back to the RC radio. The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. Individual receivers/transmitters might - violate this specification. - ''' - def __init__(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, 'RC_CHANNELS_OVERRIDE') - self._fieldnames = ['target_system', 'target_component', 'chan1_raw', 'chan2_raw', 'chan3_raw', 'chan4_raw', 'chan5_raw', 'chan6_raw', 'chan7_raw', 'chan8_raw'] - self.target_system = target_system - self.target_component = target_component - self.chan1_raw = chan1_raw - self.chan2_raw = chan2_raw - self.chan3_raw = chan3_raw - self.chan4_raw = chan4_raw - self.chan5_raw = chan5_raw - self.chan6_raw = chan6_raw - self.chan7_raw = chan7_raw - self.chan8_raw = chan8_raw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 143, struct.pack('>BBHHHHHHHH', self.target_system, self.target_component, self.chan1_raw, self.chan2_raw, self.chan3_raw, self.chan4_raw, self.chan5_raw, self.chan6_raw, self.chan7_raw, self.chan8_raw)) - -class MAVLink_global_position_int_message(MAVLink_message): - ''' - The filtered global position (e.g. fused GPS and - accelerometers). The position is in GPS-frame (right-handed, - Z-up) - ''' - def __init__(self, lat, lon, alt, vx, vy, vz): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, 'GLOBAL_POSITION_INT') - self._fieldnames = ['lat', 'lon', 'alt', 'vx', 'vy', 'vz'] - self.lat = lat - self.lon = lon - self.alt = alt - self.vx = vx - self.vy = vy - self.vz = vz - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 104, struct.pack('>iiihhh', self.lat, self.lon, self.alt, self.vx, self.vy, self.vz)) - -class MAVLink_vfr_hud_message(MAVLink_message): - ''' - Metrics typically displayed on a HUD for fixed wing aircraft - ''' - def __init__(self, airspeed, groundspeed, heading, throttle, alt, climb): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_VFR_HUD, 'VFR_HUD') - self._fieldnames = ['airspeed', 'groundspeed', 'heading', 'throttle', 'alt', 'climb'] - self.airspeed = airspeed - self.groundspeed = groundspeed - self.heading = heading - self.throttle = throttle - self.alt = alt - self.climb = climb - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 123, struct.pack('>ffhHff', self.airspeed, self.groundspeed, self.heading, self.throttle, self.alt, self.climb)) - -class MAVLink_command_message(MAVLink_message): - ''' - Send a command with up to four parameters to the MAV - ''' - def __init__(self, target_system, target_component, command, confirmation, param1, param2, param3, param4): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_COMMAND, 'COMMAND') - self._fieldnames = ['target_system', 'target_component', 'command', 'confirmation', 'param1', 'param2', 'param3', 'param4'] - self.target_system = target_system - self.target_component = target_component - self.command = command - self.confirmation = confirmation - self.param1 = param1 - self.param2 = param2 - self.param3 = param3 - self.param4 = param4 - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 131, struct.pack('>BBBBffff', self.target_system, self.target_component, self.command, self.confirmation, self.param1, self.param2, self.param3, self.param4)) - -class MAVLink_command_ack_message(MAVLink_message): - ''' - Report status of a command. Includes feedback wether the - command was executed - ''' - def __init__(self, command, result): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_COMMAND_ACK, 'COMMAND_ACK') - self._fieldnames = ['command', 'result'] - self.command = command - self.result = result - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 8, struct.pack('>ff', self.command, self.result)) - -class MAVLink_optical_flow_message(MAVLink_message): - ''' - Optical flow from a flow sensor (e.g. optical mouse sensor) - ''' - def __init__(self, time, sensor_id, flow_x, flow_y, quality, ground_distance): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_OPTICAL_FLOW, 'OPTICAL_FLOW') - self._fieldnames = ['time', 'sensor_id', 'flow_x', 'flow_y', 'quality', 'ground_distance'] - self.time = time - self.sensor_id = sensor_id - self.flow_x = flow_x - self.flow_y = flow_y - self.quality = quality - self.ground_distance = ground_distance - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 174, struct.pack('>QBhhBf', self.time, self.sensor_id, self.flow_x, self.flow_y, self.quality, self.ground_distance)) - -class MAVLink_object_detection_event_message(MAVLink_message): - ''' - Object has been detected - ''' - def __init__(self, time, object_id, type, name, quality, bearing, distance): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT, 'OBJECT_DETECTION_EVENT') - self._fieldnames = ['time', 'object_id', 'type', 'name', 'quality', 'bearing', 'distance'] - self.time = time - self.object_id = object_id - self.type = type - self.name = name - self.quality = quality - self.bearing = bearing - self.distance = distance - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 155, struct.pack('>IHB20sBff', self.time, self.object_id, self.type, self.name, self.quality, self.bearing, self.distance)) - -class MAVLink_debug_vect_message(MAVLink_message): - ''' - - ''' - def __init__(self, name, usec, x, y, z): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_DEBUG_VECT, 'DEBUG_VECT') - self._fieldnames = ['name', 'usec', 'x', 'y', 'z'] - self.name = name - self.usec = usec - self.x = x - self.y = y - self.z = z - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 178, struct.pack('>10sQfff', self.name, self.usec, self.x, self.y, self.z)) - -class MAVLink_named_value_float_message(MAVLink_message): - ''' - Send a key-value pair as float. The use of this message is - discouraged for normal packets, but a quite efficient way for - testing new messages and getting experimental debug output. - ''' - def __init__(self, name, value): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 'NAMED_VALUE_FLOAT') - self._fieldnames = ['name', 'value'] - self.name = name - self.value = value - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 224, struct.pack('>10sf', self.name, self.value)) - -class MAVLink_named_value_int_message(MAVLink_message): - ''' - Send a key-value pair as integer. The use of this message is - discouraged for normal packets, but a quite efficient way for - testing new messages and getting experimental debug output. - ''' - def __init__(self, name, value): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_NAMED_VALUE_INT, 'NAMED_VALUE_INT') - self._fieldnames = ['name', 'value'] - self.name = name - self.value = value - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 60, struct.pack('>10si', self.name, self.value)) - -class MAVLink_statustext_message(MAVLink_message): - ''' - Status text message. These messages are printed in yellow in - the COMM console of QGroundControl. WARNING: They consume - quite some bandwidth, so use only for important status and - error messages. If implemented wisely, these messages are - buffered on the MCU and sent only at a limited rate (e.g. 10 - Hz). - ''' - def __init__(self, severity, text): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_STATUSTEXT, 'STATUSTEXT') - self._fieldnames = ['severity', 'text'] - self.severity = severity - self.text = text - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 106, struct.pack('>B50s', self.severity, self.text)) - -class MAVLink_debug_message(MAVLink_message): - ''' - Send a debug value. The index is used to discriminate between - values. These values show up in the plot of QGroundControl as - DEBUG N. - ''' - def __init__(self, ind, value): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_DEBUG, 'DEBUG') - self._fieldnames = ['ind', 'value'] - self.ind = ind - self.value = value - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 7, struct.pack('>Bf', self.ind, self.value)) - - -mavlink_map = { - MAVLINK_MSG_ID_SENSOR_OFFSETS : ( '>hhhfiiffffff', MAVLink_sensor_offsets_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11], 143 ), - MAVLINK_MSG_ID_SET_MAG_OFFSETS : ( '>BBhhh', MAVLink_set_mag_offsets_message, [0, 1, 2, 3, 4], 29 ), - MAVLINK_MSG_ID_MEMINFO : ( '>HH', MAVLink_meminfo_message, [0, 1], 208 ), - MAVLINK_MSG_ID_AP_ADC : ( '>HHHHHH', MAVLink_ap_adc_message, [0, 1, 2, 3, 4, 5], 188 ), - MAVLINK_MSG_ID_DIGICAM_CONFIGURE : ( '>BBBHBBBBBBf', MAVLink_digicam_configure_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10], 118 ), - MAVLINK_MSG_ID_DIGICAM_CONTROL : ( '>BBBBbBBBBf', MAVLink_digicam_control_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 242 ), - MAVLINK_MSG_ID_MOUNT_CONFIGURE : ( '>BBBBBB', MAVLink_mount_configure_message, [0, 1, 2, 3, 4, 5], 19 ), - MAVLINK_MSG_ID_MOUNT_CONTROL : ( '>BBiiiB', MAVLink_mount_control_message, [0, 1, 2, 3, 4, 5], 97 ), - MAVLINK_MSG_ID_MOUNT_STATUS : ( '>BBiii', MAVLink_mount_status_message, [0, 1, 2, 3, 4], 233 ), - MAVLINK_MSG_ID_FENCE_POINT : ( '>BBBBff', MAVLink_fence_point_message, [0, 1, 2, 3, 4, 5], 18 ), - MAVLINK_MSG_ID_FENCE_FETCH_POINT : ( '>BBB', MAVLink_fence_fetch_point_message, [0, 1, 2], 68 ), - MAVLINK_MSG_ID_FENCE_STATUS : ( '>BHBI', MAVLink_fence_status_message, [0, 1, 2, 3], 136 ), - MAVLINK_MSG_ID_AHRS : ( '>fffffff', MAVLink_ahrs_message, [0, 1, 2, 3, 4, 5, 6], 127 ), - MAVLINK_MSG_ID_SIMSTATE : ( '>fffffffff', MAVLink_simstate_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 42 ), - MAVLINK_MSG_ID_HWSTATUS : ( '>HB', MAVLink_hwstatus_message, [0, 1], 21 ), - MAVLINK_MSG_ID_RADIO : ( '>BBBBBHH', MAVLink_radio_message, [0, 1, 2, 3, 4, 5, 6], 93 ), - MAVLINK_MSG_ID_HEARTBEAT : ( '>BBB', MAVLink_heartbeat_message, [0, 1, 2], 72 ), - MAVLINK_MSG_ID_BOOT : ( '>I', MAVLink_boot_message, [0], 39 ), - MAVLINK_MSG_ID_SYSTEM_TIME : ( '>Q', MAVLink_system_time_message, [0], 190 ), - MAVLINK_MSG_ID_PING : ( '>IBBQ', MAVLink_ping_message, [0, 1, 2, 3], 92 ), - MAVLINK_MSG_ID_SYSTEM_TIME_UTC : ( '>II', MAVLink_system_time_utc_message, [0, 1], 191 ), - MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL : ( '>BBB25s', MAVLink_change_operator_control_message, [0, 1, 2, 3], 217 ), - MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK : ( '>BBB', MAVLink_change_operator_control_ack_message, [0, 1, 2], 104 ), - MAVLINK_MSG_ID_AUTH_KEY : ( '>32s', MAVLink_auth_key_message, [0], 119 ), - MAVLINK_MSG_ID_ACTION_ACK : ( '>BB', MAVLink_action_ack_message, [0, 1], 219 ), - MAVLINK_MSG_ID_ACTION : ( '>BBB', MAVLink_action_message, [0, 1, 2], 60 ), - MAVLINK_MSG_ID_SET_MODE : ( '>BB', MAVLink_set_mode_message, [0, 1], 186 ), - MAVLINK_MSG_ID_SET_NAV_MODE : ( '>BB', MAVLink_set_nav_mode_message, [0, 1], 10 ), - MAVLINK_MSG_ID_PARAM_REQUEST_READ : ( '>BB15sh', MAVLink_param_request_read_message, [0, 1, 2, 3], 89 ), - MAVLINK_MSG_ID_PARAM_REQUEST_LIST : ( '>BB', MAVLink_param_request_list_message, [0, 1], 159 ), - MAVLINK_MSG_ID_PARAM_VALUE : ( '>15sfHH', MAVLink_param_value_message, [0, 1, 2, 3], 162 ), - MAVLINK_MSG_ID_PARAM_SET : ( '>BB15sf', MAVLink_param_set_message, [0, 1, 2, 3], 121 ), - MAVLINK_MSG_ID_GPS_RAW_INT : ( '>QBiiiffff', MAVLink_gps_raw_int_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 149 ), - MAVLINK_MSG_ID_SCALED_IMU : ( '>Qhhhhhhhhh', MAVLink_scaled_imu_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 222 ), - MAVLINK_MSG_ID_GPS_STATUS : ( '>B20s20s20s20s20s', MAVLink_gps_status_message, [0, 1, 2, 3, 4, 5], 110 ), - MAVLINK_MSG_ID_RAW_IMU : ( '>Qhhhhhhhhh', MAVLink_raw_imu_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 179 ), - MAVLINK_MSG_ID_RAW_PRESSURE : ( '>Qhhhh', MAVLink_raw_pressure_message, [0, 1, 2, 3, 4], 136 ), - MAVLINK_MSG_ID_SCALED_PRESSURE : ( '>Qffh', MAVLink_scaled_pressure_message, [0, 1, 2, 3], 229 ), - MAVLINK_MSG_ID_ATTITUDE : ( '>Qffffff', MAVLink_attitude_message, [0, 1, 2, 3, 4, 5, 6], 66 ), - MAVLINK_MSG_ID_LOCAL_POSITION : ( '>Qffffff', MAVLink_local_position_message, [0, 1, 2, 3, 4, 5, 6], 126 ), - MAVLINK_MSG_ID_GLOBAL_POSITION : ( '>Qffffff', MAVLink_global_position_message, [0, 1, 2, 3, 4, 5, 6], 147 ), - MAVLINK_MSG_ID_GPS_RAW : ( '>QBfffffff', MAVLink_gps_raw_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 185 ), - MAVLINK_MSG_ID_SYS_STATUS : ( '>BBBHHHH', MAVLink_sys_status_message, [0, 1, 2, 3, 4, 5, 6], 112 ), - MAVLINK_MSG_ID_RC_CHANNELS_RAW : ( '>HHHHHHHHB', MAVLink_rc_channels_raw_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 252 ), - MAVLINK_MSG_ID_RC_CHANNELS_SCALED : ( '>hhhhhhhhB', MAVLink_rc_channels_scaled_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 162 ), - MAVLINK_MSG_ID_SERVO_OUTPUT_RAW : ( '>HHHHHHHH', MAVLink_servo_output_raw_message, [0, 1, 2, 3, 4, 5, 6, 7], 215 ), - MAVLINK_MSG_ID_WAYPOINT : ( '>BBHBBBBfffffff', MAVLink_waypoint_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13], 128 ), - MAVLINK_MSG_ID_WAYPOINT_REQUEST : ( '>BBH', MAVLink_waypoint_request_message, [0, 1, 2], 9 ), - MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT : ( '>BBH', MAVLink_waypoint_set_current_message, [0, 1, 2], 106 ), - MAVLINK_MSG_ID_WAYPOINT_CURRENT : ( '>H', MAVLink_waypoint_current_message, [0], 101 ), - MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST : ( '>BB', MAVLink_waypoint_request_list_message, [0, 1], 213 ), - MAVLINK_MSG_ID_WAYPOINT_COUNT : ( '>BBH', MAVLink_waypoint_count_message, [0, 1, 2], 4 ), - MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL : ( '>BB', MAVLink_waypoint_clear_all_message, [0, 1], 229 ), - MAVLINK_MSG_ID_WAYPOINT_REACHED : ( '>H', MAVLink_waypoint_reached_message, [0], 21 ), - MAVLINK_MSG_ID_WAYPOINT_ACK : ( '>BBB', MAVLink_waypoint_ack_message, [0, 1, 2], 214 ), - MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN : ( '>BBiii', MAVLink_gps_set_global_origin_message, [0, 1, 2, 3, 4], 215 ), - MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET : ( '>iii', MAVLink_gps_local_origin_set_message, [0, 1, 2], 14 ), - MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET : ( '>BBffff', MAVLink_local_position_setpoint_set_message, [0, 1, 2, 3, 4, 5], 206 ), - MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT : ( '>ffff', MAVLink_local_position_setpoint_message, [0, 1, 2, 3], 50 ), - MAVLINK_MSG_ID_CONTROL_STATUS : ( '>BBBBBBBB', MAVLink_control_status_message, [0, 1, 2, 3, 4, 5, 6, 7], 157 ), - MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA : ( '>BBBffffff', MAVLink_safety_set_allowed_area_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 126 ), - MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA : ( '>Bffffff', MAVLink_safety_allowed_area_message, [0, 1, 2, 3, 4, 5, 6], 108 ), - MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST : ( '>BBffff', MAVLink_set_roll_pitch_yaw_thrust_message, [0, 1, 2, 3, 4, 5], 213 ), - MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST : ( '>BBffff', MAVLink_set_roll_pitch_yaw_speed_thrust_message, [0, 1, 2, 3, 4, 5], 95 ), - MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT : ( '>Qffff', MAVLink_roll_pitch_yaw_thrust_setpoint_message, [0, 1, 2, 3, 4], 5 ), - MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT : ( '>Qffff', MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message, [0, 1, 2, 3, 4], 127 ), - MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT : ( '>ffhhHfff', MAVLink_nav_controller_output_message, [0, 1, 2, 3, 4, 5, 6, 7], 57 ), - MAVLINK_MSG_ID_POSITION_TARGET : ( '>ffff', MAVLink_position_target_message, [0, 1, 2, 3], 126 ), - MAVLINK_MSG_ID_STATE_CORRECTION : ( '>fffffffff', MAVLink_state_correction_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 130 ), - MAVLINK_MSG_ID_SET_ALTITUDE : ( '>BI', MAVLink_set_altitude_message, [0, 1], 119 ), - MAVLINK_MSG_ID_REQUEST_DATA_STREAM : ( '>BBBHB', MAVLink_request_data_stream_message, [0, 1, 2, 3, 4], 193 ), - MAVLINK_MSG_ID_HIL_STATE : ( '>Qffffffiiihhhhhh', MAVLink_hil_state_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15], 191 ), - MAVLINK_MSG_ID_HIL_CONTROLS : ( '>QffffBB', MAVLink_hil_controls_message, [0, 1, 2, 3, 4, 5, 6], 236 ), - MAVLINK_MSG_ID_MANUAL_CONTROL : ( '>BffffBBBB', MAVLink_manual_control_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 158 ), - MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE : ( '>BBHHHHHHHH', MAVLink_rc_channels_override_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 143 ), - MAVLINK_MSG_ID_GLOBAL_POSITION_INT : ( '>iiihhh', MAVLink_global_position_int_message, [0, 1, 2, 3, 4, 5], 104 ), - MAVLINK_MSG_ID_VFR_HUD : ( '>ffhHff', MAVLink_vfr_hud_message, [0, 1, 2, 3, 4, 5], 123 ), - MAVLINK_MSG_ID_COMMAND : ( '>BBBBffff', MAVLink_command_message, [0, 1, 2, 3, 4, 5, 6, 7], 131 ), - MAVLINK_MSG_ID_COMMAND_ACK : ( '>ff', MAVLink_command_ack_message, [0, 1], 8 ), - MAVLINK_MSG_ID_OPTICAL_FLOW : ( '>QBhhBf', MAVLink_optical_flow_message, [0, 1, 2, 3, 4, 5], 174 ), - MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT : ( '>IHB20sBff', MAVLink_object_detection_event_message, [0, 1, 2, 3, 4, 5, 6], 155 ), - MAVLINK_MSG_ID_DEBUG_VECT : ( '>10sQfff', MAVLink_debug_vect_message, [0, 1, 2, 3, 4], 178 ), - MAVLINK_MSG_ID_NAMED_VALUE_FLOAT : ( '>10sf', MAVLink_named_value_float_message, [0, 1], 224 ), - MAVLINK_MSG_ID_NAMED_VALUE_INT : ( '>10si', MAVLink_named_value_int_message, [0, 1], 60 ), - MAVLINK_MSG_ID_STATUSTEXT : ( '>B50s', MAVLink_statustext_message, [0, 1], 106 ), - MAVLINK_MSG_ID_DEBUG : ( '>Bf', MAVLink_debug_message, [0, 1], 7 ), -} - -class MAVError(Exception): - '''MAVLink error class''' - def __init__(self, msg): - Exception.__init__(self, msg) - self.message = msg - -class MAVString(str): - '''NUL terminated string''' - def __init__(self, s): - str.__init__(self) - def __str__(self): - i = self.find(chr(0)) - if i == -1: - return self[:] - return self[0:i] - -class MAVLink_bad_data(MAVLink_message): - ''' - a piece of bad data in a mavlink stream - ''' - def __init__(self, data, reason): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_BAD_DATA, 'BAD_DATA') - self._fieldnames = ['data', 'reason'] - self.data = data - self.reason = reason - self._msgbuf = data - -class MAVLink(object): - '''MAVLink protocol handling class''' - def __init__(self, file, srcSystem=0, srcComponent=0): - self.seq = 0 - self.file = file - self.srcSystem = srcSystem - self.srcComponent = srcComponent - self.callback = None - self.callback_args = None - self.callback_kwargs = None - self.buf = array.array('B') - self.expected_length = 6 - self.have_prefix_error = False - self.robust_parsing = False - self.protocol_marker = 85 - self.little_endian = False - self.crc_extra = False - self.sort_fields = False - self.total_packets_sent = 0 - self.total_bytes_sent = 0 - self.total_packets_received = 0 - self.total_bytes_received = 0 - self.total_receive_errors = 0 - self.startup_time = time.time() - - def set_callback(self, callback, *args, **kwargs): - self.callback = callback - self.callback_args = args - self.callback_kwargs = kwargs - - def send(self, mavmsg): - '''send a MAVLink message''' - buf = mavmsg.pack(self) - self.file.write(buf) - self.seq = (self.seq + 1) % 255 - self.total_packets_sent += 1 - self.total_bytes_sent += len(buf) - - def bytes_needed(self): - '''return number of bytes needed for next parsing stage''' - ret = self.expected_length - len(self.buf) - if ret <= 0: - return 1 - return ret - - def parse_char(self, c): - '''input some data bytes, possibly returning a new message''' - if isinstance(c, str): - self.buf.fromstring(c) - else: - self.buf.extend(c) - self.total_bytes_received += len(c) - if len(self.buf) >= 1 and self.buf[0] != 85: - magic = self.buf[0] - self.buf = self.buf[1:] - if self.robust_parsing: - m = MAVLink_bad_data(chr(magic), "Bad prefix") - if self.callback: - self.callback(m, *self.callback_args, **self.callback_kwargs) - self.expected_length = 6 - self.total_receive_errors += 1 - return m - if self.have_prefix_error: - return None - self.have_prefix_error = True - self.total_receive_errors += 1 - raise MAVError("invalid MAVLink prefix '%s'" % magic) - self.have_prefix_error = False - if len(self.buf) >= 2: - (magic, self.expected_length) = struct.unpack('BB', self.buf[0:2]) - self.expected_length += 8 - if self.expected_length >= 8 and len(self.buf) >= self.expected_length: - mbuf = self.buf[0:self.expected_length] - self.buf = self.buf[self.expected_length:] - self.expected_length = 6 - if self.robust_parsing: - try: - m = self.decode(mbuf) - self.total_packets_received += 1 - except MAVError as reason: - m = MAVLink_bad_data(mbuf, reason.message) - self.total_receive_errors += 1 - else: - m = self.decode(mbuf) - self.total_packets_received += 1 - if self.callback: - self.callback(m, *self.callback_args, **self.callback_kwargs) - return m - return None - - def parse_buffer(self, s): - '''input some data bytes, possibly returning a list of new messages''' - m = self.parse_char(s) - if m is None: - return None - ret = [m] - while True: - m = self.parse_char("") - if m is None: - return ret - ret.append(m) - return ret - - def decode(self, msgbuf): - '''decode a buffer as a MAVLink message''' - # decode the header - try: - magic, mlen, seq, srcSystem, srcComponent, msgId = struct.unpack('cBBBBB', msgbuf[:6]) - except struct.error as emsg: - raise MAVError('Unable to unpack MAVLink header: %s' % emsg) - if ord(magic) != 85: - raise MAVError("invalid MAVLink prefix '%s'" % magic) - if mlen != len(msgbuf)-8: - raise MAVError('invalid MAVLink message length. Got %u expected %u, msgId=%u' % (len(msgbuf)-8, mlen, msgId)) - - if not msgId in mavlink_map: - raise MAVError('unknown MAVLink message ID %u' % msgId) - - # decode the payload - (fmt, type, order_map, crc_extra) = mavlink_map[msgId] - - # decode the checksum - try: - crc, = struct.unpack(' MAV. - Also used to return a point from MAV -> GCS - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - count : total number of points (for sanity checking) (uint8_t) - lat : Latitude of point (float) - lng : Longitude of point (float) - - ''' - msg = MAVLink_fence_point_message(target_system, target_component, idx, count, lat, lng) - msg.pack(self) - return msg - - def fence_point_send(self, target_system, target_component, idx, count, lat, lng): - ''' - A fence point. Used to set a point when from GCS -> MAV. - Also used to return a point from MAV -> GCS - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - count : total number of points (for sanity checking) (uint8_t) - lat : Latitude of point (float) - lng : Longitude of point (float) - - ''' - return self.send(self.fence_point_encode(target_system, target_component, idx, count, lat, lng)) - - def fence_fetch_point_encode(self, target_system, target_component, idx): - ''' - Request a current fence point from MAV - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - - ''' - msg = MAVLink_fence_fetch_point_message(target_system, target_component, idx) - msg.pack(self) - return msg - - def fence_fetch_point_send(self, target_system, target_component, idx): - ''' - Request a current fence point from MAV - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - - ''' - return self.send(self.fence_fetch_point_encode(target_system, target_component, idx)) - - def fence_status_encode(self, breach_status, breach_count, breach_type, breach_time): - ''' - Status of geo-fencing. Sent in extended status stream when - fencing enabled - - breach_status : 0 if currently inside fence, 1 if outside (uint8_t) - breach_count : number of fence breaches (uint16_t) - breach_type : last breach type (see FENCE_BREACH_* enum) (uint8_t) - breach_time : time of last breach in milliseconds since boot (uint32_t) - - ''' - msg = MAVLink_fence_status_message(breach_status, breach_count, breach_type, breach_time) - msg.pack(self) - return msg - - def fence_status_send(self, breach_status, breach_count, breach_type, breach_time): - ''' - Status of geo-fencing. Sent in extended status stream when - fencing enabled - - breach_status : 0 if currently inside fence, 1 if outside (uint8_t) - breach_count : number of fence breaches (uint16_t) - breach_type : last breach type (see FENCE_BREACH_* enum) (uint8_t) - breach_time : time of last breach in milliseconds since boot (uint32_t) - - ''' - return self.send(self.fence_status_encode(breach_status, breach_count, breach_type, breach_time)) - - def ahrs_encode(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): - ''' - Status of DCM attitude estimator - - omegaIx : X gyro drift estimate rad/s (float) - omegaIy : Y gyro drift estimate rad/s (float) - omegaIz : Z gyro drift estimate rad/s (float) - accel_weight : average accel_weight (float) - renorm_val : average renormalisation value (float) - error_rp : average error_roll_pitch value (float) - error_yaw : average error_yaw value (float) - - ''' - msg = MAVLink_ahrs_message(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw) - msg.pack(self) - return msg - - def ahrs_send(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): - ''' - Status of DCM attitude estimator - - omegaIx : X gyro drift estimate rad/s (float) - omegaIy : Y gyro drift estimate rad/s (float) - omegaIz : Z gyro drift estimate rad/s (float) - accel_weight : average accel_weight (float) - renorm_val : average renormalisation value (float) - error_rp : average error_roll_pitch value (float) - error_yaw : average error_yaw value (float) - - ''' - return self.send(self.ahrs_encode(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw)) - - def simstate_encode(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): - ''' - Status of simulation environment, if used - - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - xacc : X acceleration m/s/s (float) - yacc : Y acceleration m/s/s (float) - zacc : Z acceleration m/s/s (float) - xgyro : Angular speed around X axis rad/s (float) - ygyro : Angular speed around Y axis rad/s (float) - zgyro : Angular speed around Z axis rad/s (float) - - ''' - msg = MAVLink_simstate_message(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro) - msg.pack(self) - return msg - - def simstate_send(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): - ''' - Status of simulation environment, if used - - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - xacc : X acceleration m/s/s (float) - yacc : Y acceleration m/s/s (float) - zacc : Z acceleration m/s/s (float) - xgyro : Angular speed around X axis rad/s (float) - ygyro : Angular speed around Y axis rad/s (float) - zgyro : Angular speed around Z axis rad/s (float) - - ''' - return self.send(self.simstate_encode(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro)) - - def hwstatus_encode(self, Vcc, I2Cerr): - ''' - Status of key hardware - - Vcc : board voltage (mV) (uint16_t) - I2Cerr : I2C error count (uint8_t) - - ''' - msg = MAVLink_hwstatus_message(Vcc, I2Cerr) - msg.pack(self) - return msg - - def hwstatus_send(self, Vcc, I2Cerr): - ''' - Status of key hardware - - Vcc : board voltage (mV) (uint16_t) - I2Cerr : I2C error count (uint8_t) - - ''' - return self.send(self.hwstatus_encode(Vcc, I2Cerr)) - - def radio_encode(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): - ''' - Status generated by radio - - rssi : local signal strength (uint8_t) - remrssi : remote signal strength (uint8_t) - txbuf : how full the tx buffer is as a percentage (uint8_t) - noise : background noise level (uint8_t) - remnoise : remote background noise level (uint8_t) - rxerrors : receive errors (uint16_t) - fixed : count of error corrected packets (uint16_t) - - ''' - msg = MAVLink_radio_message(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed) - msg.pack(self) - return msg - - def radio_send(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): - ''' - Status generated by radio - - rssi : local signal strength (uint8_t) - remrssi : remote signal strength (uint8_t) - txbuf : how full the tx buffer is as a percentage (uint8_t) - noise : background noise level (uint8_t) - remnoise : remote background noise level (uint8_t) - rxerrors : receive errors (uint16_t) - fixed : count of error corrected packets (uint16_t) - - ''' - return self.send(self.radio_encode(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed)) - - def heartbeat_encode(self, type, autopilot, mavlink_version=2): - ''' - The heartbeat message shows that a system is present and responding. - The type of the MAV and Autopilot hardware allow the - receiving system to treat further messages from this - system appropriate (e.g. by laying out the user - interface based on the autopilot). - - type : Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) (uint8_t) - autopilot : Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM (uint8_t) - mavlink_version : MAVLink version (uint8_t) - - ''' - msg = MAVLink_heartbeat_message(type, autopilot, mavlink_version) - msg.pack(self) - return msg - - def heartbeat_send(self, type, autopilot, mavlink_version=2): - ''' - The heartbeat message shows that a system is present and responding. - The type of the MAV and Autopilot hardware allow the - receiving system to treat further messages from this - system appropriate (e.g. by laying out the user - interface based on the autopilot). - - type : Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) (uint8_t) - autopilot : Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM (uint8_t) - mavlink_version : MAVLink version (uint8_t) - - ''' - return self.send(self.heartbeat_encode(type, autopilot, mavlink_version)) - - def boot_encode(self, version): - ''' - The boot message indicates that a system is starting. The onboard - software version allows to keep track of onboard - soft/firmware revisions. - - version : The onboard software version (uint32_t) - - ''' - msg = MAVLink_boot_message(version) - msg.pack(self) - return msg - - def boot_send(self, version): - ''' - The boot message indicates that a system is starting. The onboard - software version allows to keep track of onboard - soft/firmware revisions. - - version : The onboard software version (uint32_t) - - ''' - return self.send(self.boot_encode(version)) - - def system_time_encode(self, time_usec): - ''' - The system time is the time of the master clock, typically the - computer clock of the main onboard computer. - - time_usec : Timestamp of the master clock in microseconds since UNIX epoch. (uint64_t) - - ''' - msg = MAVLink_system_time_message(time_usec) - msg.pack(self) - return msg - - def system_time_send(self, time_usec): - ''' - The system time is the time of the master clock, typically the - computer clock of the main onboard computer. - - time_usec : Timestamp of the master clock in microseconds since UNIX epoch. (uint64_t) - - ''' - return self.send(self.system_time_encode(time_usec)) - - def ping_encode(self, seq, target_system, target_component, time): - ''' - A ping message either requesting or responding to a ping. This allows - to measure the system latencies, including serial - port, radio modem and UDP connections. - - seq : PING sequence (uint32_t) - target_system : 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - target_component : 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - time : Unix timestamp in microseconds (uint64_t) - - ''' - msg = MAVLink_ping_message(seq, target_system, target_component, time) - msg.pack(self) - return msg - - def ping_send(self, seq, target_system, target_component, time): - ''' - A ping message either requesting or responding to a ping. This allows - to measure the system latencies, including serial - port, radio modem and UDP connections. - - seq : PING sequence (uint32_t) - target_system : 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - target_component : 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - time : Unix timestamp in microseconds (uint64_t) - - ''' - return self.send(self.ping_encode(seq, target_system, target_component, time)) - - def system_time_utc_encode(self, utc_date, utc_time): - ''' - UTC date and time from GPS module - - utc_date : GPS UTC date ddmmyy (uint32_t) - utc_time : GPS UTC time hhmmss (uint32_t) - - ''' - msg = MAVLink_system_time_utc_message(utc_date, utc_time) - msg.pack(self) - return msg - - def system_time_utc_send(self, utc_date, utc_time): - ''' - UTC date and time from GPS module - - utc_date : GPS UTC date ddmmyy (uint32_t) - utc_time : GPS UTC time hhmmss (uint32_t) - - ''' - return self.send(self.system_time_utc_encode(utc_date, utc_time)) - - def change_operator_control_encode(self, target_system, control_request, version, passkey): - ''' - Request to control this MAV - - target_system : System the GCS requests control for (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - version : 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. (uint8_t) - passkey : Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" (char) - - ''' - msg = MAVLink_change_operator_control_message(target_system, control_request, version, passkey) - msg.pack(self) - return msg - - def change_operator_control_send(self, target_system, control_request, version, passkey): - ''' - Request to control this MAV - - target_system : System the GCS requests control for (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - version : 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. (uint8_t) - passkey : Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" (char) - - ''' - return self.send(self.change_operator_control_encode(target_system, control_request, version, passkey)) - - def change_operator_control_ack_encode(self, gcs_system_id, control_request, ack): - ''' - Accept / deny control of this MAV - - gcs_system_id : ID of the GCS this message (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - ack : 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control (uint8_t) - - ''' - msg = MAVLink_change_operator_control_ack_message(gcs_system_id, control_request, ack) - msg.pack(self) - return msg - - def change_operator_control_ack_send(self, gcs_system_id, control_request, ack): - ''' - Accept / deny control of this MAV - - gcs_system_id : ID of the GCS this message (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - ack : 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control (uint8_t) - - ''' - return self.send(self.change_operator_control_ack_encode(gcs_system_id, control_request, ack)) - - def auth_key_encode(self, key): - ''' - Emit an encrypted signature / key identifying this system. PLEASE - NOTE: This protocol has been kept simple, so - transmitting the key requires an encrypted channel for - true safety. - - key : key (char) - - ''' - msg = MAVLink_auth_key_message(key) - msg.pack(self) - return msg - - def auth_key_send(self, key): - ''' - Emit an encrypted signature / key identifying this system. PLEASE - NOTE: This protocol has been kept simple, so - transmitting the key requires an encrypted channel for - true safety. - - key : key (char) - - ''' - return self.send(self.auth_key_encode(key)) - - def action_ack_encode(self, action, result): - ''' - This message acknowledges an action. IMPORTANT: The acknowledgement - can be also negative, e.g. the MAV rejects a reset - message because it is in-flight. The action ids are - defined in ENUM MAV_ACTION in - mavlink/include/mavlink_types.h - - action : The action id (uint8_t) - result : 0: Action DENIED, 1: Action executed (uint8_t) - - ''' - msg = MAVLink_action_ack_message(action, result) - msg.pack(self) - return msg - - def action_ack_send(self, action, result): - ''' - This message acknowledges an action. IMPORTANT: The acknowledgement - can be also negative, e.g. the MAV rejects a reset - message because it is in-flight. The action ids are - defined in ENUM MAV_ACTION in - mavlink/include/mavlink_types.h - - action : The action id (uint8_t) - result : 0: Action DENIED, 1: Action executed (uint8_t) - - ''' - return self.send(self.action_ack_encode(action, result)) - - def action_encode(self, target, target_component, action): - ''' - An action message allows to execute a certain onboard action. These - include liftoff, land, storing parameters too EEPROM, - shutddown, etc. The action ids are defined in ENUM - MAV_ACTION in mavlink/include/mavlink_types.h - - target : The system executing the action (uint8_t) - target_component : The component executing the action (uint8_t) - action : The action id (uint8_t) - - ''' - msg = MAVLink_action_message(target, target_component, action) - msg.pack(self) - return msg - - def action_send(self, target, target_component, action): - ''' - An action message allows to execute a certain onboard action. These - include liftoff, land, storing parameters too EEPROM, - shutddown, etc. The action ids are defined in ENUM - MAV_ACTION in mavlink/include/mavlink_types.h - - target : The system executing the action (uint8_t) - target_component : The component executing the action (uint8_t) - action : The action id (uint8_t) - - ''' - return self.send(self.action_encode(target, target_component, action)) - - def set_mode_encode(self, target, mode): - ''' - Set the system mode, as defined by enum MAV_MODE in - mavlink/include/mavlink_types.h. There is no target - component id as the mode is by definition for the - overall aircraft, not only for one component. - - target : The system setting the mode (uint8_t) - mode : The new mode (uint8_t) - - ''' - msg = MAVLink_set_mode_message(target, mode) - msg.pack(self) - return msg - - def set_mode_send(self, target, mode): - ''' - Set the system mode, as defined by enum MAV_MODE in - mavlink/include/mavlink_types.h. There is no target - component id as the mode is by definition for the - overall aircraft, not only for one component. - - target : The system setting the mode (uint8_t) - mode : The new mode (uint8_t) - - ''' - return self.send(self.set_mode_encode(target, mode)) - - def set_nav_mode_encode(self, target, nav_mode): - ''' - Set the system navigation mode, as defined by enum MAV_NAV_MODE in - mavlink/include/mavlink_types.h. The navigation mode - applies to the whole aircraft and thus all components. - - target : The system setting the mode (uint8_t) - nav_mode : The new navigation mode (uint8_t) - - ''' - msg = MAVLink_set_nav_mode_message(target, nav_mode) - msg.pack(self) - return msg - - def set_nav_mode_send(self, target, nav_mode): - ''' - Set the system navigation mode, as defined by enum MAV_NAV_MODE in - mavlink/include/mavlink_types.h. The navigation mode - applies to the whole aircraft and thus all components. - - target : The system setting the mode (uint8_t) - nav_mode : The new navigation mode (uint8_t) - - ''' - return self.send(self.set_nav_mode_encode(target, nav_mode)) - - def param_request_read_encode(self, target_system, target_component, param_id, param_index): - ''' - Request to read the onboard parameter with the param_id string id. - Onboard parameters are stored as key[const char*] -> - value[float]. This allows to send a parameter to any - other component (such as the GCS) without the need of - previous knowledge of possible parameter names. Thus - the same GCS can store different parameters for - different autopilots. See also - http://qgroundcontrol.org/parameter_interface for a - full documentation of QGroundControl and IMU code. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id (int8_t) - param_index : Parameter index. Send -1 to use the param ID field as identifier (int16_t) - - ''' - msg = MAVLink_param_request_read_message(target_system, target_component, param_id, param_index) - msg.pack(self) - return msg - - def param_request_read_send(self, target_system, target_component, param_id, param_index): - ''' - Request to read the onboard parameter with the param_id string id. - Onboard parameters are stored as key[const char*] -> - value[float]. This allows to send a parameter to any - other component (such as the GCS) without the need of - previous knowledge of possible parameter names. Thus - the same GCS can store different parameters for - different autopilots. See also - http://qgroundcontrol.org/parameter_interface for a - full documentation of QGroundControl and IMU code. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id (int8_t) - param_index : Parameter index. Send -1 to use the param ID field as identifier (int16_t) - - ''' - return self.send(self.param_request_read_encode(target_system, target_component, param_id, param_index)) - - def param_request_list_encode(self, target_system, target_component): - ''' - Request all parameters of this component. After his request, all - parameters are emitted. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - msg = MAVLink_param_request_list_message(target_system, target_component) - msg.pack(self) - return msg - - def param_request_list_send(self, target_system, target_component): - ''' - Request all parameters of this component. After his request, all - parameters are emitted. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - return self.send(self.param_request_list_encode(target_system, target_component)) - - def param_value_encode(self, param_id, param_value, param_count, param_index): - ''' - Emit the value of a onboard parameter. The inclusion of param_count - and param_index in the message allows the recipient to - keep track of received parameters and allows him to - re-request missing parameters after a loss or timeout. - - param_id : Onboard parameter id (int8_t) - param_value : Onboard parameter value (float) - param_count : Total number of onboard parameters (uint16_t) - param_index : Index of this onboard parameter (uint16_t) - - ''' - msg = MAVLink_param_value_message(param_id, param_value, param_count, param_index) - msg.pack(self) - return msg - - def param_value_send(self, param_id, param_value, param_count, param_index): - ''' - Emit the value of a onboard parameter. The inclusion of param_count - and param_index in the message allows the recipient to - keep track of received parameters and allows him to - re-request missing parameters after a loss or timeout. - - param_id : Onboard parameter id (int8_t) - param_value : Onboard parameter value (float) - param_count : Total number of onboard parameters (uint16_t) - param_index : Index of this onboard parameter (uint16_t) - - ''' - return self.send(self.param_value_encode(param_id, param_value, param_count, param_index)) - - def param_set_encode(self, target_system, target_component, param_id, param_value): - ''' - Set a parameter value TEMPORARILY to RAM. It will be reset to default - on system reboot. Send the ACTION - MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM - contents to EEPROM. IMPORTANT: The receiving component - should acknowledge the new parameter value by sending - a param_value message to all communication partners. - This will also ensure that multiple GCS all have an - up-to-date list of all parameters. If the sending GCS - did not receive a PARAM_VALUE message within its - timeout time, it should re-send the PARAM_SET message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id (int8_t) - param_value : Onboard parameter value (float) - - ''' - msg = MAVLink_param_set_message(target_system, target_component, param_id, param_value) - msg.pack(self) - return msg - - def param_set_send(self, target_system, target_component, param_id, param_value): - ''' - Set a parameter value TEMPORARILY to RAM. It will be reset to default - on system reboot. Send the ACTION - MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM - contents to EEPROM. IMPORTANT: The receiving component - should acknowledge the new parameter value by sending - a param_value message to all communication partners. - This will also ensure that multiple GCS all have an - up-to-date list of all parameters. If the sending GCS - did not receive a PARAM_VALUE message within its - timeout time, it should re-send the PARAM_SET message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id (int8_t) - param_value : Onboard parameter value (float) - - ''' - return self.send(self.param_set_encode(target_system, target_component, param_id, param_value)) - - def gps_raw_int_encode(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg): - ''' - The global position, as returned by the Global Positioning System - (GPS). This is NOT the global position estimate of the - sytem, but rather a RAW sensor value. See message - GLOBAL_POSITION for the global position estimate. - Coordinate frame is right-handed, Z-axis up (GPS - frame) - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - fix_type : 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (uint8_t) - lat : Latitude in 1E7 degrees (int32_t) - lon : Longitude in 1E7 degrees (int32_t) - alt : Altitude in 1E3 meters (millimeters) (int32_t) - eph : GPS HDOP (float) - epv : GPS VDOP (float) - v : GPS ground speed (m/s) (float) - hdg : Compass heading in degrees, 0..360 degrees (float) - - ''' - msg = MAVLink_gps_raw_int_message(usec, fix_type, lat, lon, alt, eph, epv, v, hdg) - msg.pack(self) - return msg - - def gps_raw_int_send(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg): - ''' - The global position, as returned by the Global Positioning System - (GPS). This is NOT the global position estimate of the - sytem, but rather a RAW sensor value. See message - GLOBAL_POSITION for the global position estimate. - Coordinate frame is right-handed, Z-axis up (GPS - frame) - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - fix_type : 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (uint8_t) - lat : Latitude in 1E7 degrees (int32_t) - lon : Longitude in 1E7 degrees (int32_t) - alt : Altitude in 1E3 meters (millimeters) (int32_t) - eph : GPS HDOP (float) - epv : GPS VDOP (float) - v : GPS ground speed (m/s) (float) - hdg : Compass heading in degrees, 0..360 degrees (float) - - ''' - return self.send(self.gps_raw_int_encode(usec, fix_type, lat, lon, alt, eph, epv, v, hdg)) - - def scaled_imu_encode(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should contain the scaled values to the described - units - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - xgyro : Angular speed around X axis (millirad /sec) (int16_t) - ygyro : Angular speed around Y axis (millirad /sec) (int16_t) - zgyro : Angular speed around Z axis (millirad /sec) (int16_t) - xmag : X Magnetic field (milli tesla) (int16_t) - ymag : Y Magnetic field (milli tesla) (int16_t) - zmag : Z Magnetic field (milli tesla) (int16_t) - - ''' - msg = MAVLink_scaled_imu_message(usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag) - msg.pack(self) - return msg - - def scaled_imu_send(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should contain the scaled values to the described - units - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - xgyro : Angular speed around X axis (millirad /sec) (int16_t) - ygyro : Angular speed around Y axis (millirad /sec) (int16_t) - zgyro : Angular speed around Z axis (millirad /sec) (int16_t) - xmag : X Magnetic field (milli tesla) (int16_t) - ymag : Y Magnetic field (milli tesla) (int16_t) - zmag : Z Magnetic field (milli tesla) (int16_t) - - ''' - return self.send(self.scaled_imu_encode(usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)) - - def gps_status_encode(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr): - ''' - The positioning status, as reported by GPS. This message is intended - to display status information about each satellite - visible to the receiver. See message GLOBAL_POSITION - for the global position estimate. This message can - contain information for up to 20 satellites. - - satellites_visible : Number of satellites visible (uint8_t) - satellite_prn : Global satellite ID (int8_t) - satellite_used : 0: Satellite not used, 1: used for localization (int8_t) - satellite_elevation : Elevation (0: right on top of receiver, 90: on the horizon) of satellite (int8_t) - satellite_azimuth : Direction of satellite, 0: 0 deg, 255: 360 deg. (int8_t) - satellite_snr : Signal to noise ratio of satellite (int8_t) - - ''' - msg = MAVLink_gps_status_message(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr) - msg.pack(self) - return msg - - def gps_status_send(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr): - ''' - The positioning status, as reported by GPS. This message is intended - to display status information about each satellite - visible to the receiver. See message GLOBAL_POSITION - for the global position estimate. This message can - contain information for up to 20 satellites. - - satellites_visible : Number of satellites visible (uint8_t) - satellite_prn : Global satellite ID (int8_t) - satellite_used : 0: Satellite not used, 1: used for localization (int8_t) - satellite_elevation : Elevation (0: right on top of receiver, 90: on the horizon) of satellite (int8_t) - satellite_azimuth : Direction of satellite, 0: 0 deg, 255: 360 deg. (int8_t) - satellite_snr : Signal to noise ratio of satellite (int8_t) - - ''' - return self.send(self.gps_status_encode(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr)) - - def raw_imu_encode(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should always contain the true raw values without any - scaling to allow data capture and system debugging. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - xacc : X acceleration (raw) (int16_t) - yacc : Y acceleration (raw) (int16_t) - zacc : Z acceleration (raw) (int16_t) - xgyro : Angular speed around X axis (raw) (int16_t) - ygyro : Angular speed around Y axis (raw) (int16_t) - zgyro : Angular speed around Z axis (raw) (int16_t) - xmag : X Magnetic field (raw) (int16_t) - ymag : Y Magnetic field (raw) (int16_t) - zmag : Z Magnetic field (raw) (int16_t) - - ''' - msg = MAVLink_raw_imu_message(usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag) - msg.pack(self) - return msg - - def raw_imu_send(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should always contain the true raw values without any - scaling to allow data capture and system debugging. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - xacc : X acceleration (raw) (int16_t) - yacc : Y acceleration (raw) (int16_t) - zacc : Z acceleration (raw) (int16_t) - xgyro : Angular speed around X axis (raw) (int16_t) - ygyro : Angular speed around Y axis (raw) (int16_t) - zgyro : Angular speed around Z axis (raw) (int16_t) - xmag : X Magnetic field (raw) (int16_t) - ymag : Y Magnetic field (raw) (int16_t) - zmag : Z Magnetic field (raw) (int16_t) - - ''' - return self.send(self.raw_imu_encode(usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)) - - def raw_pressure_encode(self, usec, press_abs, press_diff1, press_diff2, temperature): - ''' - The RAW pressure readings for the typical setup of one absolute - pressure and one differential pressure sensor. The - sensor values should be the raw, UNSCALED ADC values. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - press_abs : Absolute pressure (raw) (int16_t) - press_diff1 : Differential pressure 1 (raw) (int16_t) - press_diff2 : Differential pressure 2 (raw) (int16_t) - temperature : Raw Temperature measurement (raw) (int16_t) - - ''' - msg = MAVLink_raw_pressure_message(usec, press_abs, press_diff1, press_diff2, temperature) - msg.pack(self) - return msg - - def raw_pressure_send(self, usec, press_abs, press_diff1, press_diff2, temperature): - ''' - The RAW pressure readings for the typical setup of one absolute - pressure and one differential pressure sensor. The - sensor values should be the raw, UNSCALED ADC values. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - press_abs : Absolute pressure (raw) (int16_t) - press_diff1 : Differential pressure 1 (raw) (int16_t) - press_diff2 : Differential pressure 2 (raw) (int16_t) - temperature : Raw Temperature measurement (raw) (int16_t) - - ''' - return self.send(self.raw_pressure_encode(usec, press_abs, press_diff1, press_diff2, temperature)) - - def scaled_pressure_encode(self, usec, press_abs, press_diff, temperature): - ''' - The pressure readings for the typical setup of one absolute and - differential pressure sensor. The units are as - specified in each field. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - press_abs : Absolute pressure (hectopascal) (float) - press_diff : Differential pressure 1 (hectopascal) (float) - temperature : Temperature measurement (0.01 degrees celsius) (int16_t) - - ''' - msg = MAVLink_scaled_pressure_message(usec, press_abs, press_diff, temperature) - msg.pack(self) - return msg - - def scaled_pressure_send(self, usec, press_abs, press_diff, temperature): - ''' - The pressure readings for the typical setup of one absolute and - differential pressure sensor. The units are as - specified in each field. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - press_abs : Absolute pressure (hectopascal) (float) - press_diff : Differential pressure 1 (hectopascal) (float) - temperature : Temperature measurement (0.01 degrees celsius) (int16_t) - - ''' - return self.send(self.scaled_pressure_encode(usec, press_abs, press_diff, temperature)) - - def attitude_encode(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, X-front, - Y-right). - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - - ''' - msg = MAVLink_attitude_message(usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed) - msg.pack(self) - return msg - - def attitude_send(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, X-front, - Y-right). - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - - ''' - return self.send(self.attitude_encode(usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed)) - - def local_position_encode(self, usec, x, y, z, vx, vy, vz): - ''' - The filtered local position (e.g. fused computer vision and - accelerometers). Coordinate frame is right-handed, - Z-axis down (aeronautical frame) - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - x : X Position (float) - y : Y Position (float) - z : Z Position (float) - vx : X Speed (float) - vy : Y Speed (float) - vz : Z Speed (float) - - ''' - msg = MAVLink_local_position_message(usec, x, y, z, vx, vy, vz) - msg.pack(self) - return msg - - def local_position_send(self, usec, x, y, z, vx, vy, vz): - ''' - The filtered local position (e.g. fused computer vision and - accelerometers). Coordinate frame is right-handed, - Z-axis down (aeronautical frame) - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - x : X Position (float) - y : Y Position (float) - z : Z Position (float) - vx : X Speed (float) - vy : Y Speed (float) - vz : Z Speed (float) - - ''' - return self.send(self.local_position_encode(usec, x, y, z, vx, vy, vz)) - - def global_position_encode(self, usec, lat, lon, alt, vx, vy, vz): - ''' - The filtered global position (e.g. fused GPS and accelerometers). - Coordinate frame is right-handed, Z-axis up (GPS - frame) - - usec : Timestamp (microseconds since unix epoch) (uint64_t) - lat : Latitude, in degrees (float) - lon : Longitude, in degrees (float) - alt : Absolute altitude, in meters (float) - vx : X Speed (in Latitude direction, positive: going north) (float) - vy : Y Speed (in Longitude direction, positive: going east) (float) - vz : Z Speed (in Altitude direction, positive: going up) (float) - - ''' - msg = MAVLink_global_position_message(usec, lat, lon, alt, vx, vy, vz) - msg.pack(self) - return msg - - def global_position_send(self, usec, lat, lon, alt, vx, vy, vz): - ''' - The filtered global position (e.g. fused GPS and accelerometers). - Coordinate frame is right-handed, Z-axis up (GPS - frame) - - usec : Timestamp (microseconds since unix epoch) (uint64_t) - lat : Latitude, in degrees (float) - lon : Longitude, in degrees (float) - alt : Absolute altitude, in meters (float) - vx : X Speed (in Latitude direction, positive: going north) (float) - vy : Y Speed (in Longitude direction, positive: going east) (float) - vz : Z Speed (in Altitude direction, positive: going up) (float) - - ''' - return self.send(self.global_position_encode(usec, lat, lon, alt, vx, vy, vz)) - - def gps_raw_encode(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg): - ''' - The global position, as returned by the Global Positioning System - (GPS). This is NOT the global position estimate of the - sytem, but rather a RAW sensor value. See message - GLOBAL_POSITION for the global position estimate. - Coordinate frame is right-handed, Z-axis up (GPS - frame) - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - fix_type : 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (uint8_t) - lat : Latitude in degrees (float) - lon : Longitude in degrees (float) - alt : Altitude in meters (float) - eph : GPS HDOP (float) - epv : GPS VDOP (float) - v : GPS ground speed (float) - hdg : Compass heading in degrees, 0..360 degrees (float) - - ''' - msg = MAVLink_gps_raw_message(usec, fix_type, lat, lon, alt, eph, epv, v, hdg) - msg.pack(self) - return msg - - def gps_raw_send(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg): - ''' - The global position, as returned by the Global Positioning System - (GPS). This is NOT the global position estimate of the - sytem, but rather a RAW sensor value. See message - GLOBAL_POSITION for the global position estimate. - Coordinate frame is right-handed, Z-axis up (GPS - frame) - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - fix_type : 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (uint8_t) - lat : Latitude in degrees (float) - lon : Longitude in degrees (float) - alt : Altitude in meters (float) - eph : GPS HDOP (float) - epv : GPS VDOP (float) - v : GPS ground speed (float) - hdg : Compass heading in degrees, 0..360 degrees (float) - - ''' - return self.send(self.gps_raw_encode(usec, fix_type, lat, lon, alt, eph, epv, v, hdg)) - - def sys_status_encode(self, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop): - ''' - The general system state. If the system is following the MAVLink - standard, the system state is mainly defined by three - orthogonal states/modes: The system mode, which is - either LOCKED (motors shut down and locked), MANUAL - (system under RC control), GUIDED (system with - autonomous position control, position setpoint - controlled manually) or AUTO (system guided by - path/waypoint planner). The NAV_MODE defined the - current flight state: LIFTOFF (often an open-loop - maneuver), LANDING, WAYPOINTS or VECTOR. This - represents the internal navigation state machine. The - system status shows wether the system is currently - active or not and if an emergency occured. During the - CRITICAL and EMERGENCY states the MAV is still - considered to be active, but should start emergency - procedures autonomously. After a failure occured it - should first move from active to critical to allow - manual intervention and then move to emergency after a - certain timeout. - - mode : System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h (uint8_t) - nav_mode : Navigation mode, see MAV_NAV_MODE ENUM (uint8_t) - status : System status flag, see MAV_STATUS ENUM (uint8_t) - load : Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t) - vbat : Battery voltage, in millivolts (1 = 1 millivolt) (uint16_t) - battery_remaining : Remaining battery energy: (0%: 0, 100%: 1000) (uint16_t) - packet_drop : Dropped packets (packets that were corrupted on reception on the MAV) (uint16_t) - - ''' - msg = MAVLink_sys_status_message(mode, nav_mode, status, load, vbat, battery_remaining, packet_drop) - msg.pack(self) - return msg - - def sys_status_send(self, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop): - ''' - The general system state. If the system is following the MAVLink - standard, the system state is mainly defined by three - orthogonal states/modes: The system mode, which is - either LOCKED (motors shut down and locked), MANUAL - (system under RC control), GUIDED (system with - autonomous position control, position setpoint - controlled manually) or AUTO (system guided by - path/waypoint planner). The NAV_MODE defined the - current flight state: LIFTOFF (often an open-loop - maneuver), LANDING, WAYPOINTS or VECTOR. This - represents the internal navigation state machine. The - system status shows wether the system is currently - active or not and if an emergency occured. During the - CRITICAL and EMERGENCY states the MAV is still - considered to be active, but should start emergency - procedures autonomously. After a failure occured it - should first move from active to critical to allow - manual intervention and then move to emergency after a - certain timeout. - - mode : System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h (uint8_t) - nav_mode : Navigation mode, see MAV_NAV_MODE ENUM (uint8_t) - status : System status flag, see MAV_STATUS ENUM (uint8_t) - load : Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t) - vbat : Battery voltage, in millivolts (1 = 1 millivolt) (uint16_t) - battery_remaining : Remaining battery energy: (0%: 0, 100%: 1000) (uint16_t) - packet_drop : Dropped packets (packets that were corrupted on reception on the MAV) (uint16_t) - - ''' - return self.send(self.sys_status_encode(mode, nav_mode, status, load, vbat, battery_remaining, packet_drop)) - - def rc_channels_raw_encode(self, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi): - ''' - The RAW values of the RC channels received. The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. Individual receivers/transmitters - might violate this specification. - - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - msg = MAVLink_rc_channels_raw_message(chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi) - msg.pack(self) - return msg - - def rc_channels_raw_send(self, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi): - ''' - The RAW values of the RC channels received. The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. Individual receivers/transmitters - might violate this specification. - - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - return self.send(self.rc_channels_raw_encode(chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi)) - - def rc_channels_scaled_encode(self, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi): - ''' - The scaled values of the RC channels received. (-100%) -10000, (0%) 0, - (100%) 10000 - - chan1_scaled : RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan2_scaled : RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan3_scaled : RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan4_scaled : RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan5_scaled : RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan6_scaled : RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan7_scaled : RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan8_scaled : RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - msg = MAVLink_rc_channels_scaled_message(chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi) - msg.pack(self) - return msg - - def rc_channels_scaled_send(self, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi): - ''' - The scaled values of the RC channels received. (-100%) -10000, (0%) 0, - (100%) 10000 - - chan1_scaled : RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan2_scaled : RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan3_scaled : RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan4_scaled : RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan5_scaled : RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan6_scaled : RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan7_scaled : RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan8_scaled : RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - return self.send(self.rc_channels_scaled_encode(chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi)) - - def servo_output_raw_encode(self, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw): - ''' - The RAW values of the servo outputs (for RC input from the remote, use - the RC_CHANNELS messages). The standard PPM modulation - is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. - - servo1_raw : Servo output 1 value, in microseconds (uint16_t) - servo2_raw : Servo output 2 value, in microseconds (uint16_t) - servo3_raw : Servo output 3 value, in microseconds (uint16_t) - servo4_raw : Servo output 4 value, in microseconds (uint16_t) - servo5_raw : Servo output 5 value, in microseconds (uint16_t) - servo6_raw : Servo output 6 value, in microseconds (uint16_t) - servo7_raw : Servo output 7 value, in microseconds (uint16_t) - servo8_raw : Servo output 8 value, in microseconds (uint16_t) - - ''' - msg = MAVLink_servo_output_raw_message(servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw) - msg.pack(self) - return msg - - def servo_output_raw_send(self, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw): - ''' - The RAW values of the servo outputs (for RC input from the remote, use - the RC_CHANNELS messages). The standard PPM modulation - is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. - - servo1_raw : Servo output 1 value, in microseconds (uint16_t) - servo2_raw : Servo output 2 value, in microseconds (uint16_t) - servo3_raw : Servo output 3 value, in microseconds (uint16_t) - servo4_raw : Servo output 4 value, in microseconds (uint16_t) - servo5_raw : Servo output 5 value, in microseconds (uint16_t) - servo6_raw : Servo output 6 value, in microseconds (uint16_t) - servo7_raw : Servo output 7 value, in microseconds (uint16_t) - servo8_raw : Servo output 8 value, in microseconds (uint16_t) - - ''' - return self.send(self.servo_output_raw_encode(servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw)) - - def waypoint_encode(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z): - ''' - Message encoding a waypoint. This message is emitted to announce - the presence of a waypoint and to set a waypoint on - the system. The waypoint can be either in x, y, z - meters (type: LOCAL) or x:lat, y:lon, z:altitude. - Local frame is Z-down, right handed, global frame is - Z-up, right handed - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - frame : The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h (uint8_t) - command : The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs (uint8_t) - current : false:0, true:1 (uint8_t) - autocontinue : autocontinue to next wp (uint8_t) - param1 : PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters (float) - param2 : PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float) - param3 : PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float) - param4 : PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH (float) - x : PARAM5 / local: x position, global: latitude (float) - y : PARAM6 / y position: global: longitude (float) - z : PARAM7 / z position: global: altitude (float) - - ''' - msg = MAVLink_waypoint_message(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z) - msg.pack(self) - return msg - - def waypoint_send(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z): - ''' - Message encoding a waypoint. This message is emitted to announce - the presence of a waypoint and to set a waypoint on - the system. The waypoint can be either in x, y, z - meters (type: LOCAL) or x:lat, y:lon, z:altitude. - Local frame is Z-down, right handed, global frame is - Z-up, right handed - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - frame : The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h (uint8_t) - command : The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs (uint8_t) - current : false:0, true:1 (uint8_t) - autocontinue : autocontinue to next wp (uint8_t) - param1 : PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters (float) - param2 : PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float) - param3 : PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float) - param4 : PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH (float) - x : PARAM5 / local: x position, global: latitude (float) - y : PARAM6 / y position: global: longitude (float) - z : PARAM7 / z position: global: altitude (float) - - ''' - return self.send(self.waypoint_encode(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z)) - - def waypoint_request_encode(self, target_system, target_component, seq): - ''' - Request the information of the waypoint with the sequence number seq. - The response of the system to this message should be a - WAYPOINT message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_waypoint_request_message(target_system, target_component, seq) - msg.pack(self) - return msg - - def waypoint_request_send(self, target_system, target_component, seq): - ''' - Request the information of the waypoint with the sequence number seq. - The response of the system to this message should be a - WAYPOINT message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - return self.send(self.waypoint_request_encode(target_system, target_component, seq)) - - def waypoint_set_current_encode(self, target_system, target_component, seq): - ''' - Set the waypoint with sequence number seq as current waypoint. This - means that the MAV will continue to this waypoint on - the shortest path (not following the waypoints in- - between). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_waypoint_set_current_message(target_system, target_component, seq) - msg.pack(self) - return msg - - def waypoint_set_current_send(self, target_system, target_component, seq): - ''' - Set the waypoint with sequence number seq as current waypoint. This - means that the MAV will continue to this waypoint on - the shortest path (not following the waypoints in- - between). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - return self.send(self.waypoint_set_current_encode(target_system, target_component, seq)) - - def waypoint_current_encode(self, seq): - ''' - Message that announces the sequence number of the current active - waypoint. The MAV will fly towards this waypoint. - - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_waypoint_current_message(seq) - msg.pack(self) - return msg - - def waypoint_current_send(self, seq): - ''' - Message that announces the sequence number of the current active - waypoint. The MAV will fly towards this waypoint. - - seq : Sequence (uint16_t) - - ''' - return self.send(self.waypoint_current_encode(seq)) - - def waypoint_request_list_encode(self, target_system, target_component): - ''' - Request the overall list of waypoints from the system/component. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - msg = MAVLink_waypoint_request_list_message(target_system, target_component) - msg.pack(self) - return msg - - def waypoint_request_list_send(self, target_system, target_component): - ''' - Request the overall list of waypoints from the system/component. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - return self.send(self.waypoint_request_list_encode(target_system, target_component)) - - def waypoint_count_encode(self, target_system, target_component, count): - ''' - This message is emitted as response to WAYPOINT_REQUEST_LIST by the - MAV. The GCS can then request the individual waypoints - based on the knowledge of the total number of - waypoints. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - count : Number of Waypoints in the Sequence (uint16_t) - - ''' - msg = MAVLink_waypoint_count_message(target_system, target_component, count) - msg.pack(self) - return msg - - def waypoint_count_send(self, target_system, target_component, count): - ''' - This message is emitted as response to WAYPOINT_REQUEST_LIST by the - MAV. The GCS can then request the individual waypoints - based on the knowledge of the total number of - waypoints. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - count : Number of Waypoints in the Sequence (uint16_t) - - ''' - return self.send(self.waypoint_count_encode(target_system, target_component, count)) - - def waypoint_clear_all_encode(self, target_system, target_component): - ''' - Delete all waypoints at once. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - msg = MAVLink_waypoint_clear_all_message(target_system, target_component) - msg.pack(self) - return msg - - def waypoint_clear_all_send(self, target_system, target_component): - ''' - Delete all waypoints at once. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - return self.send(self.waypoint_clear_all_encode(target_system, target_component)) - - def waypoint_reached_encode(self, seq): - ''' - A certain waypoint has been reached. The system will either hold this - position (or circle on the orbit) or (if the - autocontinue on the WP was set) continue to the next - waypoint. - - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_waypoint_reached_message(seq) - msg.pack(self) - return msg - - def waypoint_reached_send(self, seq): - ''' - A certain waypoint has been reached. The system will either hold this - position (or circle on the orbit) or (if the - autocontinue on the WP was set) continue to the next - waypoint. - - seq : Sequence (uint16_t) - - ''' - return self.send(self.waypoint_reached_encode(seq)) - - def waypoint_ack_encode(self, target_system, target_component, type): - ''' - Ack message during waypoint handling. The type field states if this - message is a positive ack (type=0) or if an error - happened (type=non-zero). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - type : 0: OK, 1: Error (uint8_t) - - ''' - msg = MAVLink_waypoint_ack_message(target_system, target_component, type) - msg.pack(self) - return msg - - def waypoint_ack_send(self, target_system, target_component, type): - ''' - Ack message during waypoint handling. The type field states if this - message is a positive ack (type=0) or if an error - happened (type=non-zero). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - type : 0: OK, 1: Error (uint8_t) - - ''' - return self.send(self.waypoint_ack_encode(target_system, target_component, type)) - - def gps_set_global_origin_encode(self, target_system, target_component, latitude, longitude, altitude): - ''' - As local waypoints exist, the global waypoint reference allows to - transform between the local coordinate frame and the - global (GPS) coordinate frame. This can be necessary - when e.g. in- and outdoor settings are connected and - the MAV should move from in- to outdoor. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - latitude : global position * 1E7 (int32_t) - longitude : global position * 1E7 (int32_t) - altitude : global position * 1000 (int32_t) - - ''' - msg = MAVLink_gps_set_global_origin_message(target_system, target_component, latitude, longitude, altitude) - msg.pack(self) - return msg - - def gps_set_global_origin_send(self, target_system, target_component, latitude, longitude, altitude): - ''' - As local waypoints exist, the global waypoint reference allows to - transform between the local coordinate frame and the - global (GPS) coordinate frame. This can be necessary - when e.g. in- and outdoor settings are connected and - the MAV should move from in- to outdoor. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - latitude : global position * 1E7 (int32_t) - longitude : global position * 1E7 (int32_t) - altitude : global position * 1000 (int32_t) - - ''' - return self.send(self.gps_set_global_origin_encode(target_system, target_component, latitude, longitude, altitude)) - - def gps_local_origin_set_encode(self, latitude, longitude, altitude): - ''' - Once the MAV sets a new GPS-Local correspondence, this message - announces the origin (0,0,0) position - - latitude : Latitude (WGS84), expressed as * 1E7 (int32_t) - longitude : Longitude (WGS84), expressed as * 1E7 (int32_t) - altitude : Altitude(WGS84), expressed as * 1000 (int32_t) - - ''' - msg = MAVLink_gps_local_origin_set_message(latitude, longitude, altitude) - msg.pack(self) - return msg - - def gps_local_origin_set_send(self, latitude, longitude, altitude): - ''' - Once the MAV sets a new GPS-Local correspondence, this message - announces the origin (0,0,0) position - - latitude : Latitude (WGS84), expressed as * 1E7 (int32_t) - longitude : Longitude (WGS84), expressed as * 1E7 (int32_t) - altitude : Altitude(WGS84), expressed as * 1000 (int32_t) - - ''' - return self.send(self.gps_local_origin_set_encode(latitude, longitude, altitude)) - - def local_position_setpoint_set_encode(self, target_system, target_component, x, y, z, yaw): - ''' - Set the setpoint for a local position controller. This is the position - in local coordinates the MAV should fly to. This - message is sent by the path/waypoint planner to the - onboard position controller. As some MAVs have a - degree of freedom in yaw (e.g. all - helicopters/quadrotors), the desired yaw angle is part - of the message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - msg = MAVLink_local_position_setpoint_set_message(target_system, target_component, x, y, z, yaw) - msg.pack(self) - return msg - - def local_position_setpoint_set_send(self, target_system, target_component, x, y, z, yaw): - ''' - Set the setpoint for a local position controller. This is the position - in local coordinates the MAV should fly to. This - message is sent by the path/waypoint planner to the - onboard position controller. As some MAVs have a - degree of freedom in yaw (e.g. all - helicopters/quadrotors), the desired yaw angle is part - of the message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - return self.send(self.local_position_setpoint_set_encode(target_system, target_component, x, y, z, yaw)) - - def local_position_setpoint_encode(self, x, y, z, yaw): - ''' - Transmit the current local setpoint of the controller to other MAVs - (collision avoidance) and to the GCS. - - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - msg = MAVLink_local_position_setpoint_message(x, y, z, yaw) - msg.pack(self) - return msg - - def local_position_setpoint_send(self, x, y, z, yaw): - ''' - Transmit the current local setpoint of the controller to other MAVs - (collision avoidance) and to the GCS. - - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - return self.send(self.local_position_setpoint_encode(x, y, z, yaw)) - - def control_status_encode(self, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw): - ''' - - - position_fix : Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix (uint8_t) - vision_fix : Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix (uint8_t) - gps_fix : GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix (uint8_t) - ahrs_health : Attitude estimation health: 0: poor, 255: excellent (uint8_t) - control_att : 0: Attitude control disabled, 1: enabled (uint8_t) - control_pos_xy : 0: X, Y position control disabled, 1: enabled (uint8_t) - control_pos_z : 0: Z position control disabled, 1: enabled (uint8_t) - control_pos_yaw : 0: Yaw angle control disabled, 1: enabled (uint8_t) - - ''' - msg = MAVLink_control_status_message(position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw) - msg.pack(self) - return msg - - def control_status_send(self, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw): - ''' - - - position_fix : Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix (uint8_t) - vision_fix : Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix (uint8_t) - gps_fix : GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix (uint8_t) - ahrs_health : Attitude estimation health: 0: poor, 255: excellent (uint8_t) - control_att : 0: Attitude control disabled, 1: enabled (uint8_t) - control_pos_xy : 0: X, Y position control disabled, 1: enabled (uint8_t) - control_pos_z : 0: Z position control disabled, 1: enabled (uint8_t) - control_pos_yaw : 0: Yaw angle control disabled, 1: enabled (uint8_t) - - ''' - return self.send(self.control_status_encode(position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw)) - - def safety_set_allowed_area_encode(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Set a safety zone (volume), which is defined by two corners of a cube. - This message can be used to tell the MAV which - setpoints/waypoints to accept and which to reject. - Safety areas are often enforced by national or - competition regulations. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - msg = MAVLink_safety_set_allowed_area_message(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z) - msg.pack(self) - return msg - - def safety_set_allowed_area_send(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Set a safety zone (volume), which is defined by two corners of a cube. - This message can be used to tell the MAV which - setpoints/waypoints to accept and which to reject. - Safety areas are often enforced by national or - competition regulations. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - return self.send(self.safety_set_allowed_area_encode(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z)) - - def safety_allowed_area_encode(self, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Read out the safety zone the MAV currently assumes. - - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - msg = MAVLink_safety_allowed_area_message(frame, p1x, p1y, p1z, p2x, p2y, p2z) - msg.pack(self) - return msg - - def safety_allowed_area_send(self, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Read out the safety zone the MAV currently assumes. - - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - return self.send(self.safety_allowed_area_encode(frame, p1x, p1y, p1z, p2x, p2y, p2z)) - - def set_roll_pitch_yaw_thrust_encode(self, target_system, target_component, roll, pitch, yaw, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_set_roll_pitch_yaw_thrust_message(target_system, target_component, roll, pitch, yaw, thrust) - msg.pack(self) - return msg - - def set_roll_pitch_yaw_thrust_send(self, target_system, target_component, roll, pitch, yaw, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.set_roll_pitch_yaw_thrust_encode(target_system, target_component, roll, pitch, yaw, thrust)) - - def set_roll_pitch_yaw_speed_thrust_encode(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_set_roll_pitch_yaw_speed_thrust_message(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust) - msg.pack(self) - return msg - - def set_roll_pitch_yaw_speed_thrust_send(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.set_roll_pitch_yaw_speed_thrust_encode(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust)) - - def roll_pitch_yaw_thrust_setpoint_encode(self, time_us, roll, pitch, yaw, thrust): - ''' - Setpoint in roll, pitch, yaw currently active on the system. - - time_us : Timestamp in micro seconds since unix epoch (uint64_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_roll_pitch_yaw_thrust_setpoint_message(time_us, roll, pitch, yaw, thrust) - msg.pack(self) - return msg - - def roll_pitch_yaw_thrust_setpoint_send(self, time_us, roll, pitch, yaw, thrust): - ''' - Setpoint in roll, pitch, yaw currently active on the system. - - time_us : Timestamp in micro seconds since unix epoch (uint64_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.roll_pitch_yaw_thrust_setpoint_encode(time_us, roll, pitch, yaw, thrust)) - - def roll_pitch_yaw_speed_thrust_setpoint_encode(self, time_us, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Setpoint in rollspeed, pitchspeed, yawspeed currently active on the - system. - - time_us : Timestamp in micro seconds since unix epoch (uint64_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message(time_us, roll_speed, pitch_speed, yaw_speed, thrust) - msg.pack(self) - return msg - - def roll_pitch_yaw_speed_thrust_setpoint_send(self, time_us, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Setpoint in rollspeed, pitchspeed, yawspeed currently active on the - system. - - time_us : Timestamp in micro seconds since unix epoch (uint64_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.roll_pitch_yaw_speed_thrust_setpoint_encode(time_us, roll_speed, pitch_speed, yaw_speed, thrust)) - - def nav_controller_output_encode(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error): - ''' - Outputs of the APM navigation controller. The primary use of this - message is to check the response and signs of the - controller before actual flight and to assist with - tuning controller parameters - - nav_roll : Current desired roll in degrees (float) - nav_pitch : Current desired pitch in degrees (float) - nav_bearing : Current desired heading in degrees (int16_t) - target_bearing : Bearing to current waypoint/target in degrees (int16_t) - wp_dist : Distance to active waypoint in meters (uint16_t) - alt_error : Current altitude error in meters (float) - aspd_error : Current airspeed error in meters/second (float) - xtrack_error : Current crosstrack error on x-y plane in meters (float) - - ''' - msg = MAVLink_nav_controller_output_message(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error) - msg.pack(self) - return msg - - def nav_controller_output_send(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error): - ''' - Outputs of the APM navigation controller. The primary use of this - message is to check the response and signs of the - controller before actual flight and to assist with - tuning controller parameters - - nav_roll : Current desired roll in degrees (float) - nav_pitch : Current desired pitch in degrees (float) - nav_bearing : Current desired heading in degrees (int16_t) - target_bearing : Bearing to current waypoint/target in degrees (int16_t) - wp_dist : Distance to active waypoint in meters (uint16_t) - alt_error : Current altitude error in meters (float) - aspd_error : Current airspeed error in meters/second (float) - xtrack_error : Current crosstrack error on x-y plane in meters (float) - - ''' - return self.send(self.nav_controller_output_encode(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error)) - - def position_target_encode(self, x, y, z, yaw): - ''' - The goal position of the system. This position is the input to any - navigation or path planning algorithm and does NOT - represent the current controller setpoint. - - x : x position (float) - y : y position (float) - z : z position (float) - yaw : yaw orientation in radians, 0 = NORTH (float) - - ''' - msg = MAVLink_position_target_message(x, y, z, yaw) - msg.pack(self) - return msg - - def position_target_send(self, x, y, z, yaw): - ''' - The goal position of the system. This position is the input to any - navigation or path planning algorithm and does NOT - represent the current controller setpoint. - - x : x position (float) - y : y position (float) - z : z position (float) - yaw : yaw orientation in radians, 0 = NORTH (float) - - ''' - return self.send(self.position_target_encode(x, y, z, yaw)) - - def state_correction_encode(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr): - ''' - Corrects the systems state by adding an error correction term to the - position and velocity, and by rotating the attitude by - a correction angle. - - xErr : x position error (float) - yErr : y position error (float) - zErr : z position error (float) - rollErr : roll error (radians) (float) - pitchErr : pitch error (radians) (float) - yawErr : yaw error (radians) (float) - vxErr : x velocity (float) - vyErr : y velocity (float) - vzErr : z velocity (float) - - ''' - msg = MAVLink_state_correction_message(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr) - msg.pack(self) - return msg - - def state_correction_send(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr): - ''' - Corrects the systems state by adding an error correction term to the - position and velocity, and by rotating the attitude by - a correction angle. - - xErr : x position error (float) - yErr : y position error (float) - zErr : z position error (float) - rollErr : roll error (radians) (float) - pitchErr : pitch error (radians) (float) - yawErr : yaw error (radians) (float) - vxErr : x velocity (float) - vyErr : y velocity (float) - vzErr : z velocity (float) - - ''' - return self.send(self.state_correction_encode(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr)) - - def set_altitude_encode(self, target, mode): - ''' - - - target : The system setting the altitude (uint8_t) - mode : The new altitude in meters (uint32_t) - - ''' - msg = MAVLink_set_altitude_message(target, mode) - msg.pack(self) - return msg - - def set_altitude_send(self, target, mode): - ''' - - - target : The system setting the altitude (uint8_t) - mode : The new altitude in meters (uint32_t) - - ''' - return self.send(self.set_altitude_encode(target, mode)) - - def request_data_stream_encode(self, target_system, target_component, req_stream_id, req_message_rate, start_stop): - ''' - - - target_system : The target requested to send the message stream. (uint8_t) - target_component : The target requested to send the message stream. (uint8_t) - req_stream_id : The ID of the requested message type (uint8_t) - req_message_rate : Update rate in Hertz (uint16_t) - start_stop : 1 to start sending, 0 to stop sending. (uint8_t) - - ''' - msg = MAVLink_request_data_stream_message(target_system, target_component, req_stream_id, req_message_rate, start_stop) - msg.pack(self) - return msg - - def request_data_stream_send(self, target_system, target_component, req_stream_id, req_message_rate, start_stop): - ''' - - - target_system : The target requested to send the message stream. (uint8_t) - target_component : The target requested to send the message stream. (uint8_t) - req_stream_id : The ID of the requested message type (uint8_t) - req_message_rate : Update rate in Hertz (uint16_t) - start_stop : 1 to start sending, 0 to stop sending. (uint8_t) - - ''' - return self.send(self.request_data_stream_encode(target_system, target_component, req_stream_id, req_message_rate, start_stop)) - - def hil_state_encode(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc): - ''' - This packet is useful for high throughput applications - such as hardware in the loop simulations. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - - ''' - msg = MAVLink_hil_state_message(usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc) - msg.pack(self) - return msg - - def hil_state_send(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc): - ''' - This packet is useful for high throughput applications - such as hardware in the loop simulations. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - - ''' - return self.send(self.hil_state_encode(usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc)) - - def hil_controls_encode(self, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode): - ''' - Hardware in the loop control outputs - - time_us : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll_ailerons : Control output -3 .. 1 (float) - pitch_elevator : Control output -1 .. 1 (float) - yaw_rudder : Control output -1 .. 1 (float) - throttle : Throttle 0 .. 1 (float) - mode : System mode (MAV_MODE) (uint8_t) - nav_mode : Navigation mode (MAV_NAV_MODE) (uint8_t) - - ''' - msg = MAVLink_hil_controls_message(time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode) - msg.pack(self) - return msg - - def hil_controls_send(self, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode): - ''' - Hardware in the loop control outputs - - time_us : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll_ailerons : Control output -3 .. 1 (float) - pitch_elevator : Control output -1 .. 1 (float) - yaw_rudder : Control output -1 .. 1 (float) - throttle : Throttle 0 .. 1 (float) - mode : System mode (MAV_MODE) (uint8_t) - nav_mode : Navigation mode (MAV_NAV_MODE) (uint8_t) - - ''' - return self.send(self.hil_controls_encode(time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode)) - - def manual_control_encode(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual): - ''' - - - target : The system to be controlled (uint8_t) - roll : roll (float) - pitch : pitch (float) - yaw : yaw (float) - thrust : thrust (float) - roll_manual : roll control enabled auto:0, manual:1 (uint8_t) - pitch_manual : pitch auto:0, manual:1 (uint8_t) - yaw_manual : yaw auto:0, manual:1 (uint8_t) - thrust_manual : thrust auto:0, manual:1 (uint8_t) - - ''' - msg = MAVLink_manual_control_message(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual) - msg.pack(self) - return msg - - def manual_control_send(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual): - ''' - - - target : The system to be controlled (uint8_t) - roll : roll (float) - pitch : pitch (float) - yaw : yaw (float) - thrust : thrust (float) - roll_manual : roll control enabled auto:0, manual:1 (uint8_t) - pitch_manual : pitch auto:0, manual:1 (uint8_t) - yaw_manual : yaw auto:0, manual:1 (uint8_t) - thrust_manual : thrust auto:0, manual:1 (uint8_t) - - ''' - return self.send(self.manual_control_encode(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual)) - - def rc_channels_override_encode(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw): - ''' - The RAW values of the RC channels sent to the MAV to override info - received from the RC radio. A value of -1 means no - change to that channel. A value of 0 means control of - that channel should be released back to the RC radio. - The standard PPM modulation is as follows: 1000 - microseconds: 0%, 2000 microseconds: 100%. Individual - receivers/transmitters might violate this - specification. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - - ''' - msg = MAVLink_rc_channels_override_message(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw) - msg.pack(self) - return msg - - def rc_channels_override_send(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw): - ''' - The RAW values of the RC channels sent to the MAV to override info - received from the RC radio. A value of -1 means no - change to that channel. A value of 0 means control of - that channel should be released back to the RC radio. - The standard PPM modulation is as follows: 1000 - microseconds: 0%, 2000 microseconds: 100%. Individual - receivers/transmitters might violate this - specification. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - - ''' - return self.send(self.rc_channels_override_encode(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw)) - - def global_position_int_encode(self, lat, lon, alt, vx, vy, vz): - ''' - The filtered global position (e.g. fused GPS and accelerometers). The - position is in GPS-frame (right-handed, Z-up) - - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - - ''' - msg = MAVLink_global_position_int_message(lat, lon, alt, vx, vy, vz) - msg.pack(self) - return msg - - def global_position_int_send(self, lat, lon, alt, vx, vy, vz): - ''' - The filtered global position (e.g. fused GPS and accelerometers). The - position is in GPS-frame (right-handed, Z-up) - - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - - ''' - return self.send(self.global_position_int_encode(lat, lon, alt, vx, vy, vz)) - - def vfr_hud_encode(self, airspeed, groundspeed, heading, throttle, alt, climb): - ''' - Metrics typically displayed on a HUD for fixed wing aircraft - - airspeed : Current airspeed in m/s (float) - groundspeed : Current ground speed in m/s (float) - heading : Current heading in degrees, in compass units (0..360, 0=north) (int16_t) - throttle : Current throttle setting in integer percent, 0 to 100 (uint16_t) - alt : Current altitude (MSL), in meters (float) - climb : Current climb rate in meters/second (float) - - ''' - msg = MAVLink_vfr_hud_message(airspeed, groundspeed, heading, throttle, alt, climb) - msg.pack(self) - return msg - - def vfr_hud_send(self, airspeed, groundspeed, heading, throttle, alt, climb): - ''' - Metrics typically displayed on a HUD for fixed wing aircraft - - airspeed : Current airspeed in m/s (float) - groundspeed : Current ground speed in m/s (float) - heading : Current heading in degrees, in compass units (0..360, 0=north) (int16_t) - throttle : Current throttle setting in integer percent, 0 to 100 (uint16_t) - alt : Current altitude (MSL), in meters (float) - climb : Current climb rate in meters/second (float) - - ''' - return self.send(self.vfr_hud_encode(airspeed, groundspeed, heading, throttle, alt, climb)) - - def command_encode(self, target_system, target_component, command, confirmation, param1, param2, param3, param4): - ''' - Send a command with up to four parameters to the MAV - - target_system : System which should execute the command (uint8_t) - target_component : Component which should execute the command, 0 for all components (uint8_t) - command : Command ID, as defined by MAV_CMD enum. (uint8_t) - confirmation : 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) (uint8_t) - param1 : Parameter 1, as defined by MAV_CMD enum. (float) - param2 : Parameter 2, as defined by MAV_CMD enum. (float) - param3 : Parameter 3, as defined by MAV_CMD enum. (float) - param4 : Parameter 4, as defined by MAV_CMD enum. (float) - - ''' - msg = MAVLink_command_message(target_system, target_component, command, confirmation, param1, param2, param3, param4) - msg.pack(self) - return msg - - def command_send(self, target_system, target_component, command, confirmation, param1, param2, param3, param4): - ''' - Send a command with up to four parameters to the MAV - - target_system : System which should execute the command (uint8_t) - target_component : Component which should execute the command, 0 for all components (uint8_t) - command : Command ID, as defined by MAV_CMD enum. (uint8_t) - confirmation : 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) (uint8_t) - param1 : Parameter 1, as defined by MAV_CMD enum. (float) - param2 : Parameter 2, as defined by MAV_CMD enum. (float) - param3 : Parameter 3, as defined by MAV_CMD enum. (float) - param4 : Parameter 4, as defined by MAV_CMD enum. (float) - - ''' - return self.send(self.command_encode(target_system, target_component, command, confirmation, param1, param2, param3, param4)) - - def command_ack_encode(self, command, result): - ''' - Report status of a command. Includes feedback wether the command was - executed - - command : Current airspeed in m/s (float) - result : 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION (float) - - ''' - msg = MAVLink_command_ack_message(command, result) - msg.pack(self) - return msg - - def command_ack_send(self, command, result): - ''' - Report status of a command. Includes feedback wether the command was - executed - - command : Current airspeed in m/s (float) - result : 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION (float) - - ''' - return self.send(self.command_ack_encode(command, result)) - - def optical_flow_encode(self, time, sensor_id, flow_x, flow_y, quality, ground_distance): - ''' - Optical flow from a flow sensor (e.g. optical mouse sensor) - - time : Timestamp (UNIX) (uint64_t) - sensor_id : Sensor ID (uint8_t) - flow_x : Flow in pixels in x-sensor direction (int16_t) - flow_y : Flow in pixels in y-sensor direction (int16_t) - quality : Optical flow quality / confidence. 0: bad, 255: maximum quality (uint8_t) - ground_distance : Ground distance in meters (float) - - ''' - msg = MAVLink_optical_flow_message(time, sensor_id, flow_x, flow_y, quality, ground_distance) - msg.pack(self) - return msg - - def optical_flow_send(self, time, sensor_id, flow_x, flow_y, quality, ground_distance): - ''' - Optical flow from a flow sensor (e.g. optical mouse sensor) - - time : Timestamp (UNIX) (uint64_t) - sensor_id : Sensor ID (uint8_t) - flow_x : Flow in pixels in x-sensor direction (int16_t) - flow_y : Flow in pixels in y-sensor direction (int16_t) - quality : Optical flow quality / confidence. 0: bad, 255: maximum quality (uint8_t) - ground_distance : Ground distance in meters (float) - - ''' - return self.send(self.optical_flow_encode(time, sensor_id, flow_x, flow_y, quality, ground_distance)) - - def object_detection_event_encode(self, time, object_id, type, name, quality, bearing, distance): - ''' - Object has been detected - - time : Timestamp in milliseconds since system boot (uint32_t) - object_id : Object ID (uint16_t) - type : Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal (uint8_t) - name : Name of the object as defined by the detector (char) - quality : Detection quality / confidence. 0: bad, 255: maximum confidence (uint8_t) - bearing : Angle of the object with respect to the body frame in NED coordinates in radians. 0: front (float) - distance : Ground distance in meters (float) - - ''' - msg = MAVLink_object_detection_event_message(time, object_id, type, name, quality, bearing, distance) - msg.pack(self) - return msg - - def object_detection_event_send(self, time, object_id, type, name, quality, bearing, distance): - ''' - Object has been detected - - time : Timestamp in milliseconds since system boot (uint32_t) - object_id : Object ID (uint16_t) - type : Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal (uint8_t) - name : Name of the object as defined by the detector (char) - quality : Detection quality / confidence. 0: bad, 255: maximum confidence (uint8_t) - bearing : Angle of the object with respect to the body frame in NED coordinates in radians. 0: front (float) - distance : Ground distance in meters (float) - - ''' - return self.send(self.object_detection_event_encode(time, object_id, type, name, quality, bearing, distance)) - - def debug_vect_encode(self, name, usec, x, y, z): - ''' - - - name : Name (char) - usec : Timestamp (uint64_t) - x : x (float) - y : y (float) - z : z (float) - - ''' - msg = MAVLink_debug_vect_message(name, usec, x, y, z) - msg.pack(self) - return msg - - def debug_vect_send(self, name, usec, x, y, z): - ''' - - - name : Name (char) - usec : Timestamp (uint64_t) - x : x (float) - y : y (float) - z : z (float) - - ''' - return self.send(self.debug_vect_encode(name, usec, x, y, z)) - - def named_value_float_encode(self, name, value): - ''' - Send a key-value pair as float. The use of this message is discouraged - for normal packets, but a quite efficient way for - testing new messages and getting experimental debug - output. - - name : Name of the debug variable (char) - value : Floating point value (float) - - ''' - msg = MAVLink_named_value_float_message(name, value) - msg.pack(self) - return msg - - def named_value_float_send(self, name, value): - ''' - Send a key-value pair as float. The use of this message is discouraged - for normal packets, but a quite efficient way for - testing new messages and getting experimental debug - output. - - name : Name of the debug variable (char) - value : Floating point value (float) - - ''' - return self.send(self.named_value_float_encode(name, value)) - - def named_value_int_encode(self, name, value): - ''' - Send a key-value pair as integer. The use of this message is - discouraged for normal packets, but a quite efficient - way for testing new messages and getting experimental - debug output. - - name : Name of the debug variable (char) - value : Signed integer value (int32_t) - - ''' - msg = MAVLink_named_value_int_message(name, value) - msg.pack(self) - return msg - - def named_value_int_send(self, name, value): - ''' - Send a key-value pair as integer. The use of this message is - discouraged for normal packets, but a quite efficient - way for testing new messages and getting experimental - debug output. - - name : Name of the debug variable (char) - value : Signed integer value (int32_t) - - ''' - return self.send(self.named_value_int_encode(name, value)) - - def statustext_encode(self, severity, text): - ''' - Status text message. These messages are printed in yellow in the COMM - console of QGroundControl. WARNING: They consume quite - some bandwidth, so use only for important status and - error messages. If implemented wisely, these messages - are buffered on the MCU and sent only at a limited - rate (e.g. 10 Hz). - - severity : Severity of status, 0 = info message, 255 = critical fault (uint8_t) - text : Status text message, without null termination character (int8_t) - - ''' - msg = MAVLink_statustext_message(severity, text) - msg.pack(self) - return msg - - def statustext_send(self, severity, text): - ''' - Status text message. These messages are printed in yellow in the COMM - console of QGroundControl. WARNING: They consume quite - some bandwidth, so use only for important status and - error messages. If implemented wisely, these messages - are buffered on the MCU and sent only at a limited - rate (e.g. 10 Hz). - - severity : Severity of status, 0 = info message, 255 = critical fault (uint8_t) - text : Status text message, without null termination character (int8_t) - - ''' - return self.send(self.statustext_encode(severity, text)) - - def debug_encode(self, ind, value): - ''' - Send a debug value. The index is used to discriminate between values. - These values show up in the plot of QGroundControl as - DEBUG N. - - ind : index of debug variable (uint8_t) - value : DEBUG value (float) - - ''' - msg = MAVLink_debug_message(ind, value) - msg.pack(self) - return msg - - def debug_send(self, ind, value): - ''' - Send a debug value. The index is used to discriminate between values. - These values show up in the plot of QGroundControl as - DEBUG N. - - ind : index of debug variable (uint8_t) - value : DEBUG value (float) - - ''' - return self.send(self.debug_encode(ind, value)) - diff --git a/mavlink/share/pyshared/pymavlink/mavlinkv10.py b/mavlink/share/pyshared/pymavlink/mavlinkv10.py deleted file mode 100644 index a87e8e904..000000000 --- a/mavlink/share/pyshared/pymavlink/mavlinkv10.py +++ /dev/null @@ -1,5394 +0,0 @@ -''' -MAVLink protocol implementation (auto-generated by mavgen.py) - -Generated from: ardupilotmega.xml,common.xml - -Note: this file has been auto-generated. DO NOT EDIT -''' - -import struct, array, mavutil, time - -WIRE_PROTOCOL_VERSION = "1.0" - -class MAVLink_header(object): - '''MAVLink message header''' - def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0): - self.mlen = mlen - self.seq = seq - self.srcSystem = srcSystem - self.srcComponent = srcComponent - self.msgId = msgId - - def pack(self): - return struct.pack('BBBBBB', 254, self.mlen, self.seq, - self.srcSystem, self.srcComponent, self.msgId) - -class MAVLink_message(object): - '''base MAVLink message class''' - def __init__(self, msgId, name): - self._header = MAVLink_header(msgId) - self._payload = None - self._msgbuf = None - self._crc = None - self._fieldnames = [] - self._type = name - - def get_msgbuf(self): - return self._msgbuf - - def get_header(self): - return self._header - - def get_payload(self): - return self._payload - - def get_crc(self): - return self._crc - - def get_fieldnames(self): - return self._fieldnames - - def get_type(self): - return self._type - - def get_msgId(self): - return self._header.msgId - - def get_srcSystem(self): - return self._header.srcSystem - - def get_srcComponent(self): - return self._header.srcComponent - - def get_seq(self): - return self._header.seq - - def __str__(self): - ret = '%s {' % self._type - for a in self._fieldnames: - v = getattr(self, a) - ret += '%s : %s, ' % (a, v) - ret = ret[0:-2] + '}' - return ret - - def pack(self, mav, crc_extra, payload): - self._payload = payload - self._header = MAVLink_header(self._header.msgId, len(payload), mav.seq, - mav.srcSystem, mav.srcComponent) - self._msgbuf = self._header.pack() + payload - crc = mavutil.x25crc(self._msgbuf[1:]) - if True: # using CRC extra - crc.accumulate(chr(crc_extra)) - self._crc = crc.crc - self._msgbuf += struct.pack(' MAV. Also used to return a point from MAV -> GCS - ''' - def __init__(self, target_system, target_component, idx, count, lat, lng): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_POINT, 'FENCE_POINT') - self._fieldnames = ['target_system', 'target_component', 'idx', 'count', 'lat', 'lng'] - self.target_system = target_system - self.target_component = target_component - self.idx = idx - self.count = count - self.lat = lat - self.lng = lng - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 78, struct.pack(' - value[float]. This allows to send a parameter to any other - component (such as the GCS) without the need of previous - knowledge of possible parameter names. Thus the same GCS can - store different parameters for different autopilots. See also - http://qgroundcontrol.org/parameter_interface for a full - documentation of QGroundControl and IMU code. - ''' - def __init__(self, target_system, target_component, param_id, param_index): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_REQUEST_READ, 'PARAM_REQUEST_READ') - self._fieldnames = ['target_system', 'target_component', 'param_id', 'param_index'] - self.target_system = target_system - self.target_component = target_component - self.param_id = param_id - self.param_index = param_index - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 214, struct.pack('= 1 and self.buf[0] != 254: - magic = self.buf[0] - self.buf = self.buf[1:] - if self.robust_parsing: - m = MAVLink_bad_data(chr(magic), "Bad prefix") - if self.callback: - self.callback(m, *self.callback_args, **self.callback_kwargs) - self.expected_length = 6 - self.total_receive_errors += 1 - return m - if self.have_prefix_error: - return None - self.have_prefix_error = True - self.total_receive_errors += 1 - raise MAVError("invalid MAVLink prefix '%s'" % magic) - self.have_prefix_error = False - if len(self.buf) >= 2: - (magic, self.expected_length) = struct.unpack('BB', self.buf[0:2]) - self.expected_length += 8 - if self.expected_length >= 8 and len(self.buf) >= self.expected_length: - mbuf = self.buf[0:self.expected_length] - self.buf = self.buf[self.expected_length:] - self.expected_length = 6 - if self.robust_parsing: - try: - m = self.decode(mbuf) - self.total_packets_received += 1 - except MAVError as reason: - m = MAVLink_bad_data(mbuf, reason.message) - self.total_receive_errors += 1 - else: - m = self.decode(mbuf) - self.total_packets_received += 1 - if self.callback: - self.callback(m, *self.callback_args, **self.callback_kwargs) - return m - return None - - def parse_buffer(self, s): - '''input some data bytes, possibly returning a list of new messages''' - m = self.parse_char(s) - if m is None: - return None - ret = [m] - while True: - m = self.parse_char("") - if m is None: - return ret - ret.append(m) - return ret - - def decode(self, msgbuf): - '''decode a buffer as a MAVLink message''' - # decode the header - try: - magic, mlen, seq, srcSystem, srcComponent, msgId = struct.unpack('cBBBBB', msgbuf[:6]) - except struct.error as emsg: - raise MAVError('Unable to unpack MAVLink header: %s' % emsg) - if ord(magic) != 254: - raise MAVError("invalid MAVLink prefix '%s'" % magic) - if mlen != len(msgbuf)-8: - raise MAVError('invalid MAVLink message length. Got %u expected %u, msgId=%u' % (len(msgbuf)-8, mlen, msgId)) - - if not msgId in mavlink_map: - raise MAVError('unknown MAVLink message ID %u' % msgId) - - # decode the payload - (fmt, type, order_map, crc_extra) = mavlink_map[msgId] - - # decode the checksum - try: - crc, = struct.unpack(' MAV. - Also used to return a point from MAV -> GCS - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - count : total number of points (for sanity checking) (uint8_t) - lat : Latitude of point (float) - lng : Longitude of point (float) - - ''' - msg = MAVLink_fence_point_message(target_system, target_component, idx, count, lat, lng) - msg.pack(self) - return msg - - def fence_point_send(self, target_system, target_component, idx, count, lat, lng): - ''' - A fence point. Used to set a point when from GCS -> MAV. - Also used to return a point from MAV -> GCS - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - count : total number of points (for sanity checking) (uint8_t) - lat : Latitude of point (float) - lng : Longitude of point (float) - - ''' - return self.send(self.fence_point_encode(target_system, target_component, idx, count, lat, lng)) - - def fence_fetch_point_encode(self, target_system, target_component, idx): - ''' - Request a current fence point from MAV - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - - ''' - msg = MAVLink_fence_fetch_point_message(target_system, target_component, idx) - msg.pack(self) - return msg - - def fence_fetch_point_send(self, target_system, target_component, idx): - ''' - Request a current fence point from MAV - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - - ''' - return self.send(self.fence_fetch_point_encode(target_system, target_component, idx)) - - def fence_status_encode(self, breach_status, breach_count, breach_type, breach_time): - ''' - Status of geo-fencing. Sent in extended status stream when - fencing enabled - - breach_status : 0 if currently inside fence, 1 if outside (uint8_t) - breach_count : number of fence breaches (uint16_t) - breach_type : last breach type (see FENCE_BREACH_* enum) (uint8_t) - breach_time : time of last breach in milliseconds since boot (uint32_t) - - ''' - msg = MAVLink_fence_status_message(breach_status, breach_count, breach_type, breach_time) - msg.pack(self) - return msg - - def fence_status_send(self, breach_status, breach_count, breach_type, breach_time): - ''' - Status of geo-fencing. Sent in extended status stream when - fencing enabled - - breach_status : 0 if currently inside fence, 1 if outside (uint8_t) - breach_count : number of fence breaches (uint16_t) - breach_type : last breach type (see FENCE_BREACH_* enum) (uint8_t) - breach_time : time of last breach in milliseconds since boot (uint32_t) - - ''' - return self.send(self.fence_status_encode(breach_status, breach_count, breach_type, breach_time)) - - def ahrs_encode(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): - ''' - Status of DCM attitude estimator - - omegaIx : X gyro drift estimate rad/s (float) - omegaIy : Y gyro drift estimate rad/s (float) - omegaIz : Z gyro drift estimate rad/s (float) - accel_weight : average accel_weight (float) - renorm_val : average renormalisation value (float) - error_rp : average error_roll_pitch value (float) - error_yaw : average error_yaw value (float) - - ''' - msg = MAVLink_ahrs_message(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw) - msg.pack(self) - return msg - - def ahrs_send(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): - ''' - Status of DCM attitude estimator - - omegaIx : X gyro drift estimate rad/s (float) - omegaIy : Y gyro drift estimate rad/s (float) - omegaIz : Z gyro drift estimate rad/s (float) - accel_weight : average accel_weight (float) - renorm_val : average renormalisation value (float) - error_rp : average error_roll_pitch value (float) - error_yaw : average error_yaw value (float) - - ''' - return self.send(self.ahrs_encode(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw)) - - def simstate_encode(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): - ''' - Status of simulation environment, if used - - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - xacc : X acceleration m/s/s (float) - yacc : Y acceleration m/s/s (float) - zacc : Z acceleration m/s/s (float) - xgyro : Angular speed around X axis rad/s (float) - ygyro : Angular speed around Y axis rad/s (float) - zgyro : Angular speed around Z axis rad/s (float) - - ''' - msg = MAVLink_simstate_message(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro) - msg.pack(self) - return msg - - def simstate_send(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): - ''' - Status of simulation environment, if used - - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - xacc : X acceleration m/s/s (float) - yacc : Y acceleration m/s/s (float) - zacc : Z acceleration m/s/s (float) - xgyro : Angular speed around X axis rad/s (float) - ygyro : Angular speed around Y axis rad/s (float) - zgyro : Angular speed around Z axis rad/s (float) - - ''' - return self.send(self.simstate_encode(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro)) - - def hwstatus_encode(self, Vcc, I2Cerr): - ''' - Status of key hardware - - Vcc : board voltage (mV) (uint16_t) - I2Cerr : I2C error count (uint8_t) - - ''' - msg = MAVLink_hwstatus_message(Vcc, I2Cerr) - msg.pack(self) - return msg - - def hwstatus_send(self, Vcc, I2Cerr): - ''' - Status of key hardware - - Vcc : board voltage (mV) (uint16_t) - I2Cerr : I2C error count (uint8_t) - - ''' - return self.send(self.hwstatus_encode(Vcc, I2Cerr)) - - def radio_encode(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): - ''' - Status generated by radio - - rssi : local signal strength (uint8_t) - remrssi : remote signal strength (uint8_t) - txbuf : how full the tx buffer is as a percentage (uint8_t) - noise : background noise level (uint8_t) - remnoise : remote background noise level (uint8_t) - rxerrors : receive errors (uint16_t) - fixed : count of error corrected packets (uint16_t) - - ''' - msg = MAVLink_radio_message(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed) - msg.pack(self) - return msg - - def radio_send(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): - ''' - Status generated by radio - - rssi : local signal strength (uint8_t) - remrssi : remote signal strength (uint8_t) - txbuf : how full the tx buffer is as a percentage (uint8_t) - noise : background noise level (uint8_t) - remnoise : remote background noise level (uint8_t) - rxerrors : receive errors (uint16_t) - fixed : count of error corrected packets (uint16_t) - - ''' - return self.send(self.radio_encode(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed)) - - def heartbeat_encode(self, type, autopilot, base_mode, custom_mode, system_status, mavlink_version=3): - ''' - The heartbeat message shows that a system is present and responding. - The type of the MAV and Autopilot hardware allow the - receiving system to treat further messages from this - system appropriate (e.g. by laying out the user - interface based on the autopilot). - - type : Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) (uint8_t) - autopilot : Autopilot type / class. defined in MAV_CLASS ENUM (uint8_t) - base_mode : System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h (uint8_t) - custom_mode : Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific. (uint32_t) - system_status : System status flag, see MAV_STATUS ENUM (uint8_t) - mavlink_version : MAVLink version (uint8_t) - - ''' - msg = MAVLink_heartbeat_message(type, autopilot, base_mode, custom_mode, system_status, mavlink_version) - msg.pack(self) - return msg - - def heartbeat_send(self, type, autopilot, base_mode, custom_mode, system_status, mavlink_version=3): - ''' - The heartbeat message shows that a system is present and responding. - The type of the MAV and Autopilot hardware allow the - receiving system to treat further messages from this - system appropriate (e.g. by laying out the user - interface based on the autopilot). - - type : Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) (uint8_t) - autopilot : Autopilot type / class. defined in MAV_CLASS ENUM (uint8_t) - base_mode : System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h (uint8_t) - custom_mode : Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific. (uint32_t) - system_status : System status flag, see MAV_STATUS ENUM (uint8_t) - mavlink_version : MAVLink version (uint8_t) - - ''' - return self.send(self.heartbeat_encode(type, autopilot, base_mode, custom_mode, system_status, mavlink_version)) - - def sys_status_encode(self, onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4): - ''' - The general system state. If the system is following the MAVLink - standard, the system state is mainly defined by three - orthogonal states/modes: The system mode, which is - either LOCKED (motors shut down and locked), MANUAL - (system under RC control), GUIDED (system with - autonomous position control, position setpoint - controlled manually) or AUTO (system guided by - path/waypoint planner). The NAV_MODE defined the - current flight state: LIFTOFF (often an open-loop - maneuver), LANDING, WAYPOINTS or VECTOR. This - represents the internal navigation state machine. The - system status shows wether the system is currently - active or not and if an emergency occured. During the - CRITICAL and EMERGENCY states the MAV is still - considered to be active, but should start emergency - procedures autonomously. After a failure occured it - should first move from active to critical to allow - manual intervention and then move to emergency after a - certain timeout. - - onboard_control_sensors_present : Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t) - onboard_control_sensors_enabled : Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t) - onboard_control_sensors_health : Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t) - load : Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t) - voltage_battery : Battery voltage, in millivolts (1 = 1 millivolt) (uint16_t) - current_battery : Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current (int16_t) - battery_remaining : Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery (int8_t) - drop_rate_comm : Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) (uint16_t) - errors_comm : Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) (uint16_t) - errors_count1 : Autopilot-specific errors (uint16_t) - errors_count2 : Autopilot-specific errors (uint16_t) - errors_count3 : Autopilot-specific errors (uint16_t) - errors_count4 : Autopilot-specific errors (uint16_t) - - ''' - msg = MAVLink_sys_status_message(onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4) - msg.pack(self) - return msg - - def sys_status_send(self, onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4): - ''' - The general system state. If the system is following the MAVLink - standard, the system state is mainly defined by three - orthogonal states/modes: The system mode, which is - either LOCKED (motors shut down and locked), MANUAL - (system under RC control), GUIDED (system with - autonomous position control, position setpoint - controlled manually) or AUTO (system guided by - path/waypoint planner). The NAV_MODE defined the - current flight state: LIFTOFF (often an open-loop - maneuver), LANDING, WAYPOINTS or VECTOR. This - represents the internal navigation state machine. The - system status shows wether the system is currently - active or not and if an emergency occured. During the - CRITICAL and EMERGENCY states the MAV is still - considered to be active, but should start emergency - procedures autonomously. After a failure occured it - should first move from active to critical to allow - manual intervention and then move to emergency after a - certain timeout. - - onboard_control_sensors_present : Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t) - onboard_control_sensors_enabled : Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t) - onboard_control_sensors_health : Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t) - load : Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t) - voltage_battery : Battery voltage, in millivolts (1 = 1 millivolt) (uint16_t) - current_battery : Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current (int16_t) - battery_remaining : Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery (int8_t) - drop_rate_comm : Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) (uint16_t) - errors_comm : Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) (uint16_t) - errors_count1 : Autopilot-specific errors (uint16_t) - errors_count2 : Autopilot-specific errors (uint16_t) - errors_count3 : Autopilot-specific errors (uint16_t) - errors_count4 : Autopilot-specific errors (uint16_t) - - ''' - return self.send(self.sys_status_encode(onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4)) - - def system_time_encode(self, time_unix_usec, time_boot_ms): - ''' - The system time is the time of the master clock, typically the - computer clock of the main onboard computer. - - time_unix_usec : Timestamp of the master clock in microseconds since UNIX epoch. (uint64_t) - time_boot_ms : Timestamp of the component clock since boot time in milliseconds. (uint32_t) - - ''' - msg = MAVLink_system_time_message(time_unix_usec, time_boot_ms) - msg.pack(self) - return msg - - def system_time_send(self, time_unix_usec, time_boot_ms): - ''' - The system time is the time of the master clock, typically the - computer clock of the main onboard computer. - - time_unix_usec : Timestamp of the master clock in microseconds since UNIX epoch. (uint64_t) - time_boot_ms : Timestamp of the component clock since boot time in milliseconds. (uint32_t) - - ''' - return self.send(self.system_time_encode(time_unix_usec, time_boot_ms)) - - def ping_encode(self, time_usec, seq, target_system, target_component): - ''' - A ping message either requesting or responding to a ping. This allows - to measure the system latencies, including serial - port, radio modem and UDP connections. - - time_usec : Unix timestamp in microseconds (uint64_t) - seq : PING sequence (uint32_t) - target_system : 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - target_component : 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - - ''' - msg = MAVLink_ping_message(time_usec, seq, target_system, target_component) - msg.pack(self) - return msg - - def ping_send(self, time_usec, seq, target_system, target_component): - ''' - A ping message either requesting or responding to a ping. This allows - to measure the system latencies, including serial - port, radio modem and UDP connections. - - time_usec : Unix timestamp in microseconds (uint64_t) - seq : PING sequence (uint32_t) - target_system : 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - target_component : 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - - ''' - return self.send(self.ping_encode(time_usec, seq, target_system, target_component)) - - def change_operator_control_encode(self, target_system, control_request, version, passkey): - ''' - Request to control this MAV - - target_system : System the GCS requests control for (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - version : 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. (uint8_t) - passkey : Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" (char) - - ''' - msg = MAVLink_change_operator_control_message(target_system, control_request, version, passkey) - msg.pack(self) - return msg - - def change_operator_control_send(self, target_system, control_request, version, passkey): - ''' - Request to control this MAV - - target_system : System the GCS requests control for (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - version : 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. (uint8_t) - passkey : Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" (char) - - ''' - return self.send(self.change_operator_control_encode(target_system, control_request, version, passkey)) - - def change_operator_control_ack_encode(self, gcs_system_id, control_request, ack): - ''' - Accept / deny control of this MAV - - gcs_system_id : ID of the GCS this message (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - ack : 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control (uint8_t) - - ''' - msg = MAVLink_change_operator_control_ack_message(gcs_system_id, control_request, ack) - msg.pack(self) - return msg - - def change_operator_control_ack_send(self, gcs_system_id, control_request, ack): - ''' - Accept / deny control of this MAV - - gcs_system_id : ID of the GCS this message (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - ack : 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control (uint8_t) - - ''' - return self.send(self.change_operator_control_ack_encode(gcs_system_id, control_request, ack)) - - def auth_key_encode(self, key): - ''' - Emit an encrypted signature / key identifying this system. PLEASE - NOTE: This protocol has been kept simple, so - transmitting the key requires an encrypted channel for - true safety. - - key : key (char) - - ''' - msg = MAVLink_auth_key_message(key) - msg.pack(self) - return msg - - def auth_key_send(self, key): - ''' - Emit an encrypted signature / key identifying this system. PLEASE - NOTE: This protocol has been kept simple, so - transmitting the key requires an encrypted channel for - true safety. - - key : key (char) - - ''' - return self.send(self.auth_key_encode(key)) - - def set_mode_encode(self, target_system, base_mode, custom_mode): - ''' - Set the system mode, as defined by enum MAV_MODE. There is no target - component id as the mode is by definition for the - overall aircraft, not only for one component. - - target_system : The system setting the mode (uint8_t) - base_mode : The new base mode (uint8_t) - custom_mode : The new autopilot-specific mode. This field can be ignored by an autopilot. (uint32_t) - - ''' - msg = MAVLink_set_mode_message(target_system, base_mode, custom_mode) - msg.pack(self) - return msg - - def set_mode_send(self, target_system, base_mode, custom_mode): - ''' - Set the system mode, as defined by enum MAV_MODE. There is no target - component id as the mode is by definition for the - overall aircraft, not only for one component. - - target_system : The system setting the mode (uint8_t) - base_mode : The new base mode (uint8_t) - custom_mode : The new autopilot-specific mode. This field can be ignored by an autopilot. (uint32_t) - - ''' - return self.send(self.set_mode_encode(target_system, base_mode, custom_mode)) - - def param_request_read_encode(self, target_system, target_component, param_id, param_index): - ''' - Request to read the onboard parameter with the param_id string id. - Onboard parameters are stored as key[const char*] -> - value[float]. This allows to send a parameter to any - other component (such as the GCS) without the need of - previous knowledge of possible parameter names. Thus - the same GCS can store different parameters for - different autopilots. See also - http://qgroundcontrol.org/parameter_interface for a - full documentation of QGroundControl and IMU code. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id (char) - param_index : Parameter index. Send -1 to use the param ID field as identifier (int16_t) - - ''' - msg = MAVLink_param_request_read_message(target_system, target_component, param_id, param_index) - msg.pack(self) - return msg - - def param_request_read_send(self, target_system, target_component, param_id, param_index): - ''' - Request to read the onboard parameter with the param_id string id. - Onboard parameters are stored as key[const char*] -> - value[float]. This allows to send a parameter to any - other component (such as the GCS) without the need of - previous knowledge of possible parameter names. Thus - the same GCS can store different parameters for - different autopilots. See also - http://qgroundcontrol.org/parameter_interface for a - full documentation of QGroundControl and IMU code. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id (char) - param_index : Parameter index. Send -1 to use the param ID field as identifier (int16_t) - - ''' - return self.send(self.param_request_read_encode(target_system, target_component, param_id, param_index)) - - def param_request_list_encode(self, target_system, target_component): - ''' - Request all parameters of this component. After his request, all - parameters are emitted. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - msg = MAVLink_param_request_list_message(target_system, target_component) - msg.pack(self) - return msg - - def param_request_list_send(self, target_system, target_component): - ''' - Request all parameters of this component. After his request, all - parameters are emitted. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - return self.send(self.param_request_list_encode(target_system, target_component)) - - def param_value_encode(self, param_id, param_value, param_type, param_count, param_index): - ''' - Emit the value of a onboard parameter. The inclusion of param_count - and param_index in the message allows the recipient to - keep track of received parameters and allows him to - re-request missing parameters after a loss or timeout. - - param_id : Onboard parameter id (char) - param_value : Onboard parameter value (float) - param_type : Onboard parameter type: see MAV_VAR enum (uint8_t) - param_count : Total number of onboard parameters (uint16_t) - param_index : Index of this onboard parameter (uint16_t) - - ''' - msg = MAVLink_param_value_message(param_id, param_value, param_type, param_count, param_index) - msg.pack(self) - return msg - - def param_value_send(self, param_id, param_value, param_type, param_count, param_index): - ''' - Emit the value of a onboard parameter. The inclusion of param_count - and param_index in the message allows the recipient to - keep track of received parameters and allows him to - re-request missing parameters after a loss or timeout. - - param_id : Onboard parameter id (char) - param_value : Onboard parameter value (float) - param_type : Onboard parameter type: see MAV_VAR enum (uint8_t) - param_count : Total number of onboard parameters (uint16_t) - param_index : Index of this onboard parameter (uint16_t) - - ''' - return self.send(self.param_value_encode(param_id, param_value, param_type, param_count, param_index)) - - def param_set_encode(self, target_system, target_component, param_id, param_value, param_type): - ''' - Set a parameter value TEMPORARILY to RAM. It will be reset to default - on system reboot. Send the ACTION - MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM - contents to EEPROM. IMPORTANT: The receiving component - should acknowledge the new parameter value by sending - a param_value message to all communication partners. - This will also ensure that multiple GCS all have an - up-to-date list of all parameters. If the sending GCS - did not receive a PARAM_VALUE message within its - timeout time, it should re-send the PARAM_SET message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id (char) - param_value : Onboard parameter value (float) - param_type : Onboard parameter type: see MAV_VAR enum (uint8_t) - - ''' - msg = MAVLink_param_set_message(target_system, target_component, param_id, param_value, param_type) - msg.pack(self) - return msg - - def param_set_send(self, target_system, target_component, param_id, param_value, param_type): - ''' - Set a parameter value TEMPORARILY to RAM. It will be reset to default - on system reboot. Send the ACTION - MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM - contents to EEPROM. IMPORTANT: The receiving component - should acknowledge the new parameter value by sending - a param_value message to all communication partners. - This will also ensure that multiple GCS all have an - up-to-date list of all parameters. If the sending GCS - did not receive a PARAM_VALUE message within its - timeout time, it should re-send the PARAM_SET message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id (char) - param_value : Onboard parameter value (float) - param_type : Onboard parameter type: see MAV_VAR enum (uint8_t) - - ''' - return self.send(self.param_set_encode(target_system, target_component, param_id, param_value, param_type)) - - def gps_raw_int_encode(self, time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible): - ''' - The global position, as returned by the Global Positioning System - (GPS). This is NOT the global position - estimate of the sytem, but rather a RAW sensor value. - See message GLOBAL_POSITION for the global position - estimate. Coordinate frame is right-handed, Z-axis up - (GPS frame) - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - fix_type : 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (uint8_t) - lat : Latitude in 1E7 degrees (int32_t) - lon : Longitude in 1E7 degrees (int32_t) - alt : Altitude in 1E3 meters (millimeters) above MSL (int32_t) - eph : GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 (uint16_t) - epv : GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 (uint16_t) - vel : GPS ground speed (m/s * 100). If unknown, set to: 65535 (uint16_t) - cog : Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 (uint16_t) - satellites_visible : Number of satellites visible. If unknown, set to 255 (uint8_t) - - ''' - msg = MAVLink_gps_raw_int_message(time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible) - msg.pack(self) - return msg - - def gps_raw_int_send(self, time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible): - ''' - The global position, as returned by the Global Positioning System - (GPS). This is NOT the global position - estimate of the sytem, but rather a RAW sensor value. - See message GLOBAL_POSITION for the global position - estimate. Coordinate frame is right-handed, Z-axis up - (GPS frame) - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - fix_type : 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (uint8_t) - lat : Latitude in 1E7 degrees (int32_t) - lon : Longitude in 1E7 degrees (int32_t) - alt : Altitude in 1E3 meters (millimeters) above MSL (int32_t) - eph : GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 (uint16_t) - epv : GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 (uint16_t) - vel : GPS ground speed (m/s * 100). If unknown, set to: 65535 (uint16_t) - cog : Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 (uint16_t) - satellites_visible : Number of satellites visible. If unknown, set to 255 (uint8_t) - - ''' - return self.send(self.gps_raw_int_encode(time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible)) - - def gps_status_encode(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr): - ''' - The positioning status, as reported by GPS. This message is intended - to display status information about each satellite - visible to the receiver. See message GLOBAL_POSITION - for the global position estimate. This message can - contain information for up to 20 satellites. - - satellites_visible : Number of satellites visible (uint8_t) - satellite_prn : Global satellite ID (uint8_t) - satellite_used : 0: Satellite not used, 1: used for localization (uint8_t) - satellite_elevation : Elevation (0: right on top of receiver, 90: on the horizon) of satellite (uint8_t) - satellite_azimuth : Direction of satellite, 0: 0 deg, 255: 360 deg. (uint8_t) - satellite_snr : Signal to noise ratio of satellite (uint8_t) - - ''' - msg = MAVLink_gps_status_message(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr) - msg.pack(self) - return msg - - def gps_status_send(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr): - ''' - The positioning status, as reported by GPS. This message is intended - to display status information about each satellite - visible to the receiver. See message GLOBAL_POSITION - for the global position estimate. This message can - contain information for up to 20 satellites. - - satellites_visible : Number of satellites visible (uint8_t) - satellite_prn : Global satellite ID (uint8_t) - satellite_used : 0: Satellite not used, 1: used for localization (uint8_t) - satellite_elevation : Elevation (0: right on top of receiver, 90: on the horizon) of satellite (uint8_t) - satellite_azimuth : Direction of satellite, 0: 0 deg, 255: 360 deg. (uint8_t) - satellite_snr : Signal to noise ratio of satellite (uint8_t) - - ''' - return self.send(self.gps_status_encode(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr)) - - def scaled_imu_encode(self, time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should contain the scaled values to the described - units - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - xgyro : Angular speed around X axis (millirad /sec) (int16_t) - ygyro : Angular speed around Y axis (millirad /sec) (int16_t) - zgyro : Angular speed around Z axis (millirad /sec) (int16_t) - xmag : X Magnetic field (milli tesla) (int16_t) - ymag : Y Magnetic field (milli tesla) (int16_t) - zmag : Z Magnetic field (milli tesla) (int16_t) - - ''' - msg = MAVLink_scaled_imu_message(time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag) - msg.pack(self) - return msg - - def scaled_imu_send(self, time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should contain the scaled values to the described - units - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - xgyro : Angular speed around X axis (millirad /sec) (int16_t) - ygyro : Angular speed around Y axis (millirad /sec) (int16_t) - zgyro : Angular speed around Z axis (millirad /sec) (int16_t) - xmag : X Magnetic field (milli tesla) (int16_t) - ymag : Y Magnetic field (milli tesla) (int16_t) - zmag : Z Magnetic field (milli tesla) (int16_t) - - ''' - return self.send(self.scaled_imu_encode(time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)) - - def raw_imu_encode(self, time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should always contain the true raw values without any - scaling to allow data capture and system debugging. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - xacc : X acceleration (raw) (int16_t) - yacc : Y acceleration (raw) (int16_t) - zacc : Z acceleration (raw) (int16_t) - xgyro : Angular speed around X axis (raw) (int16_t) - ygyro : Angular speed around Y axis (raw) (int16_t) - zgyro : Angular speed around Z axis (raw) (int16_t) - xmag : X Magnetic field (raw) (int16_t) - ymag : Y Magnetic field (raw) (int16_t) - zmag : Z Magnetic field (raw) (int16_t) - - ''' - msg = MAVLink_raw_imu_message(time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag) - msg.pack(self) - return msg - - def raw_imu_send(self, time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should always contain the true raw values without any - scaling to allow data capture and system debugging. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - xacc : X acceleration (raw) (int16_t) - yacc : Y acceleration (raw) (int16_t) - zacc : Z acceleration (raw) (int16_t) - xgyro : Angular speed around X axis (raw) (int16_t) - ygyro : Angular speed around Y axis (raw) (int16_t) - zgyro : Angular speed around Z axis (raw) (int16_t) - xmag : X Magnetic field (raw) (int16_t) - ymag : Y Magnetic field (raw) (int16_t) - zmag : Z Magnetic field (raw) (int16_t) - - ''' - return self.send(self.raw_imu_encode(time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)) - - def raw_pressure_encode(self, time_usec, press_abs, press_diff1, press_diff2, temperature): - ''' - The RAW pressure readings for the typical setup of one absolute - pressure and one differential pressure sensor. The - sensor values should be the raw, UNSCALED ADC values. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - press_abs : Absolute pressure (raw) (int16_t) - press_diff1 : Differential pressure 1 (raw) (int16_t) - press_diff2 : Differential pressure 2 (raw) (int16_t) - temperature : Raw Temperature measurement (raw) (int16_t) - - ''' - msg = MAVLink_raw_pressure_message(time_usec, press_abs, press_diff1, press_diff2, temperature) - msg.pack(self) - return msg - - def raw_pressure_send(self, time_usec, press_abs, press_diff1, press_diff2, temperature): - ''' - The RAW pressure readings for the typical setup of one absolute - pressure and one differential pressure sensor. The - sensor values should be the raw, UNSCALED ADC values. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - press_abs : Absolute pressure (raw) (int16_t) - press_diff1 : Differential pressure 1 (raw) (int16_t) - press_diff2 : Differential pressure 2 (raw) (int16_t) - temperature : Raw Temperature measurement (raw) (int16_t) - - ''' - return self.send(self.raw_pressure_encode(time_usec, press_abs, press_diff1, press_diff2, temperature)) - - def scaled_pressure_encode(self, time_boot_ms, press_abs, press_diff, temperature): - ''' - The pressure readings for the typical setup of one absolute and - differential pressure sensor. The units are as - specified in each field. - - time_boot_ms : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint32_t) - press_abs : Absolute pressure (hectopascal) (float) - press_diff : Differential pressure 1 (hectopascal) (float) - temperature : Temperature measurement (0.01 degrees celsius) (int16_t) - - ''' - msg = MAVLink_scaled_pressure_message(time_boot_ms, press_abs, press_diff, temperature) - msg.pack(self) - return msg - - def scaled_pressure_send(self, time_boot_ms, press_abs, press_diff, temperature): - ''' - The pressure readings for the typical setup of one absolute and - differential pressure sensor. The units are as - specified in each field. - - time_boot_ms : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint32_t) - press_abs : Absolute pressure (hectopascal) (float) - press_diff : Differential pressure 1 (hectopascal) (float) - temperature : Temperature measurement (0.01 degrees celsius) (int16_t) - - ''' - return self.send(self.scaled_pressure_encode(time_boot_ms, press_abs, press_diff, temperature)) - - def attitude_encode(self, time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, X-front, - Y-right). - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - - ''' - msg = MAVLink_attitude_message(time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed) - msg.pack(self) - return msg - - def attitude_send(self, time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, X-front, - Y-right). - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - - ''' - return self.send(self.attitude_encode(time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed)) - - def attitude_quaternion_encode(self, time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, X-front, - Y-right), expressed as quaternion. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - q1 : Quaternion component 1 (float) - q2 : Quaternion component 2 (float) - q3 : Quaternion component 3 (float) - q4 : Quaternion component 4 (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - - ''' - msg = MAVLink_attitude_quaternion_message(time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed) - msg.pack(self) - return msg - - def attitude_quaternion_send(self, time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, X-front, - Y-right), expressed as quaternion. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - q1 : Quaternion component 1 (float) - q2 : Quaternion component 2 (float) - q3 : Quaternion component 3 (float) - q4 : Quaternion component 4 (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - - ''' - return self.send(self.attitude_quaternion_encode(time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed)) - - def local_position_ned_encode(self, time_boot_ms, x, y, z, vx, vy, vz): - ''' - The filtered local position (e.g. fused computer vision and - accelerometers). Coordinate frame is right-handed, - Z-axis down (aeronautical frame, NED / north-east-down - convention) - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - x : X Position (float) - y : Y Position (float) - z : Z Position (float) - vx : X Speed (float) - vy : Y Speed (float) - vz : Z Speed (float) - - ''' - msg = MAVLink_local_position_ned_message(time_boot_ms, x, y, z, vx, vy, vz) - msg.pack(self) - return msg - - def local_position_ned_send(self, time_boot_ms, x, y, z, vx, vy, vz): - ''' - The filtered local position (e.g. fused computer vision and - accelerometers). Coordinate frame is right-handed, - Z-axis down (aeronautical frame, NED / north-east-down - convention) - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - x : X Position (float) - y : Y Position (float) - z : Z Position (float) - vx : X Speed (float) - vy : Y Speed (float) - vz : Z Speed (float) - - ''' - return self.send(self.local_position_ned_encode(time_boot_ms, x, y, z, vx, vy, vz)) - - def global_position_int_encode(self, time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg): - ''' - The filtered global position (e.g. fused GPS and accelerometers). The - position is in GPS-frame (right-handed, Z-up). It - is designed as scaled integer message since the - resolution of float is not sufficient. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters), above MSL (int32_t) - relative_alt : Altitude above ground in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - hdg : Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 (uint16_t) - - ''' - msg = MAVLink_global_position_int_message(time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg) - msg.pack(self) - return msg - - def global_position_int_send(self, time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg): - ''' - The filtered global position (e.g. fused GPS and accelerometers). The - position is in GPS-frame (right-handed, Z-up). It - is designed as scaled integer message since the - resolution of float is not sufficient. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters), above MSL (int32_t) - relative_alt : Altitude above ground in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - hdg : Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 (uint16_t) - - ''' - return self.send(self.global_position_int_encode(time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg)) - - def rc_channels_scaled_encode(self, time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi): - ''' - The scaled values of the RC channels received. (-100%) -10000, (0%) 0, - (100%) 10000 - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - port : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t) - chan1_scaled : RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan2_scaled : RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan3_scaled : RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan4_scaled : RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan5_scaled : RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan6_scaled : RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan7_scaled : RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan8_scaled : RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - msg = MAVLink_rc_channels_scaled_message(time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi) - msg.pack(self) - return msg - - def rc_channels_scaled_send(self, time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi): - ''' - The scaled values of the RC channels received. (-100%) -10000, (0%) 0, - (100%) 10000 - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - port : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t) - chan1_scaled : RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan2_scaled : RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan3_scaled : RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan4_scaled : RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan5_scaled : RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan6_scaled : RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan7_scaled : RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan8_scaled : RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - return self.send(self.rc_channels_scaled_encode(time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi)) - - def rc_channels_raw_encode(self, time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi): - ''' - The RAW values of the RC channels received. The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. Individual receivers/transmitters - might violate this specification. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - port : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - msg = MAVLink_rc_channels_raw_message(time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi) - msg.pack(self) - return msg - - def rc_channels_raw_send(self, time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi): - ''' - The RAW values of the RC channels received. The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. Individual receivers/transmitters - might violate this specification. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - port : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - return self.send(self.rc_channels_raw_encode(time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi)) - - def servo_output_raw_encode(self, time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw): - ''' - The RAW values of the servo outputs (for RC input from the remote, use - the RC_CHANNELS messages). The standard PPM modulation - is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. - - time_usec : Timestamp (since UNIX epoch or microseconds since system boot) (uint32_t) - port : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t) - servo1_raw : Servo output 1 value, in microseconds (uint16_t) - servo2_raw : Servo output 2 value, in microseconds (uint16_t) - servo3_raw : Servo output 3 value, in microseconds (uint16_t) - servo4_raw : Servo output 4 value, in microseconds (uint16_t) - servo5_raw : Servo output 5 value, in microseconds (uint16_t) - servo6_raw : Servo output 6 value, in microseconds (uint16_t) - servo7_raw : Servo output 7 value, in microseconds (uint16_t) - servo8_raw : Servo output 8 value, in microseconds (uint16_t) - - ''' - msg = MAVLink_servo_output_raw_message(time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw) - msg.pack(self) - return msg - - def servo_output_raw_send(self, time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw): - ''' - The RAW values of the servo outputs (for RC input from the remote, use - the RC_CHANNELS messages). The standard PPM modulation - is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. - - time_usec : Timestamp (since UNIX epoch or microseconds since system boot) (uint32_t) - port : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t) - servo1_raw : Servo output 1 value, in microseconds (uint16_t) - servo2_raw : Servo output 2 value, in microseconds (uint16_t) - servo3_raw : Servo output 3 value, in microseconds (uint16_t) - servo4_raw : Servo output 4 value, in microseconds (uint16_t) - servo5_raw : Servo output 5 value, in microseconds (uint16_t) - servo6_raw : Servo output 6 value, in microseconds (uint16_t) - servo7_raw : Servo output 7 value, in microseconds (uint16_t) - servo8_raw : Servo output 8 value, in microseconds (uint16_t) - - ''' - return self.send(self.servo_output_raw_encode(time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw)) - - def mission_request_partial_list_encode(self, target_system, target_component, start_index, end_index): - ''' - Request the overall list of MISSIONs from the system/component. - http://qgroundcontrol.org/mavlink/waypoint_protocol - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - start_index : Start index, 0 by default (int16_t) - end_index : End index, -1 by default (-1: send list to end). Else a valid index of the list (int16_t) - - ''' - msg = MAVLink_mission_request_partial_list_message(target_system, target_component, start_index, end_index) - msg.pack(self) - return msg - - def mission_request_partial_list_send(self, target_system, target_component, start_index, end_index): - ''' - Request the overall list of MISSIONs from the system/component. - http://qgroundcontrol.org/mavlink/waypoint_protocol - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - start_index : Start index, 0 by default (int16_t) - end_index : End index, -1 by default (-1: send list to end). Else a valid index of the list (int16_t) - - ''' - return self.send(self.mission_request_partial_list_encode(target_system, target_component, start_index, end_index)) - - def mission_write_partial_list_encode(self, target_system, target_component, start_index, end_index): - ''' - This message is sent to the MAV to write a partial list. If start - index == end index, only one item will be transmitted - / updated. If the start index is NOT 0 and above the - current list size, this request should be REJECTED! - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - start_index : Start index, 0 by default and smaller / equal to the largest index of the current onboard list. (int16_t) - end_index : End index, equal or greater than start index. (int16_t) - - ''' - msg = MAVLink_mission_write_partial_list_message(target_system, target_component, start_index, end_index) - msg.pack(self) - return msg - - def mission_write_partial_list_send(self, target_system, target_component, start_index, end_index): - ''' - This message is sent to the MAV to write a partial list. If start - index == end index, only one item will be transmitted - / updated. If the start index is NOT 0 and above the - current list size, this request should be REJECTED! - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - start_index : Start index, 0 by default and smaller / equal to the largest index of the current onboard list. (int16_t) - end_index : End index, equal or greater than start index. (int16_t) - - ''' - return self.send(self.mission_write_partial_list_encode(target_system, target_component, start_index, end_index)) - - def mission_item_encode(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z): - ''' - Message encoding a mission item. This message is emitted to announce - the presence of a mission item and to set a mission - item on the system. The mission item can be either in - x, y, z meters (type: LOCAL) or x:lat, y:lon, - z:altitude. Local frame is Z-down, right handed (NED), - global frame is Z-up, right handed (ENU). - http://qgroundcontrol.org/mavlink/waypoint_protocol - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - frame : The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h (uint8_t) - command : The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs (uint16_t) - current : false:0, true:1 (uint8_t) - autocontinue : autocontinue to next wp (uint8_t) - param1 : PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters (float) - param2 : PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float) - param3 : PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float) - param4 : PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH (float) - x : PARAM5 / local: x position, global: latitude (float) - y : PARAM6 / y position: global: longitude (float) - z : PARAM7 / z position: global: altitude (float) - - ''' - msg = MAVLink_mission_item_message(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z) - msg.pack(self) - return msg - - def mission_item_send(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z): - ''' - Message encoding a mission item. This message is emitted to announce - the presence of a mission item and to set a mission - item on the system. The mission item can be either in - x, y, z meters (type: LOCAL) or x:lat, y:lon, - z:altitude. Local frame is Z-down, right handed (NED), - global frame is Z-up, right handed (ENU). - http://qgroundcontrol.org/mavlink/waypoint_protocol - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - frame : The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h (uint8_t) - command : The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs (uint16_t) - current : false:0, true:1 (uint8_t) - autocontinue : autocontinue to next wp (uint8_t) - param1 : PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters (float) - param2 : PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float) - param3 : PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float) - param4 : PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH (float) - x : PARAM5 / local: x position, global: latitude (float) - y : PARAM6 / y position: global: longitude (float) - z : PARAM7 / z position: global: altitude (float) - - ''' - return self.send(self.mission_item_encode(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z)) - - def mission_request_encode(self, target_system, target_component, seq): - ''' - Request the information of the mission item with the sequence number - seq. The response of the system to this message should - be a MISSION_ITEM message. - http://qgroundcontrol.org/mavlink/waypoint_protocol - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_mission_request_message(target_system, target_component, seq) - msg.pack(self) - return msg - - def mission_request_send(self, target_system, target_component, seq): - ''' - Request the information of the mission item with the sequence number - seq. The response of the system to this message should - be a MISSION_ITEM message. - http://qgroundcontrol.org/mavlink/waypoint_protocol - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - return self.send(self.mission_request_encode(target_system, target_component, seq)) - - def mission_set_current_encode(self, target_system, target_component, seq): - ''' - Set the mission item with sequence number seq as current item. This - means that the MAV will continue to this mission item - on the shortest path (not following the mission items - in-between). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_mission_set_current_message(target_system, target_component, seq) - msg.pack(self) - return msg - - def mission_set_current_send(self, target_system, target_component, seq): - ''' - Set the mission item with sequence number seq as current item. This - means that the MAV will continue to this mission item - on the shortest path (not following the mission items - in-between). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - return self.send(self.mission_set_current_encode(target_system, target_component, seq)) - - def mission_current_encode(self, seq): - ''' - Message that announces the sequence number of the current active - mission item. The MAV will fly towards this mission - item. - - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_mission_current_message(seq) - msg.pack(self) - return msg - - def mission_current_send(self, seq): - ''' - Message that announces the sequence number of the current active - mission item. The MAV will fly towards this mission - item. - - seq : Sequence (uint16_t) - - ''' - return self.send(self.mission_current_encode(seq)) - - def mission_request_list_encode(self, target_system, target_component): - ''' - Request the overall list of mission items from the system/component. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - msg = MAVLink_mission_request_list_message(target_system, target_component) - msg.pack(self) - return msg - - def mission_request_list_send(self, target_system, target_component): - ''' - Request the overall list of mission items from the system/component. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - return self.send(self.mission_request_list_encode(target_system, target_component)) - - def mission_count_encode(self, target_system, target_component, count): - ''' - This message is emitted as response to MISSION_REQUEST_LIST by the MAV - and to initiate a write transaction. The GCS can then - request the individual mission item based on the - knowledge of the total number of MISSIONs. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - count : Number of mission items in the sequence (uint16_t) - - ''' - msg = MAVLink_mission_count_message(target_system, target_component, count) - msg.pack(self) - return msg - - def mission_count_send(self, target_system, target_component, count): - ''' - This message is emitted as response to MISSION_REQUEST_LIST by the MAV - and to initiate a write transaction. The GCS can then - request the individual mission item based on the - knowledge of the total number of MISSIONs. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - count : Number of mission items in the sequence (uint16_t) - - ''' - return self.send(self.mission_count_encode(target_system, target_component, count)) - - def mission_clear_all_encode(self, target_system, target_component): - ''' - Delete all mission items at once. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - msg = MAVLink_mission_clear_all_message(target_system, target_component) - msg.pack(self) - return msg - - def mission_clear_all_send(self, target_system, target_component): - ''' - Delete all mission items at once. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - return self.send(self.mission_clear_all_encode(target_system, target_component)) - - def mission_item_reached_encode(self, seq): - ''' - A certain mission item has been reached. The system will either hold - this position (or circle on the orbit) or (if the - autocontinue on the WP was set) continue to the next - MISSION. - - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_mission_item_reached_message(seq) - msg.pack(self) - return msg - - def mission_item_reached_send(self, seq): - ''' - A certain mission item has been reached. The system will either hold - this position (or circle on the orbit) or (if the - autocontinue on the WP was set) continue to the next - MISSION. - - seq : Sequence (uint16_t) - - ''' - return self.send(self.mission_item_reached_encode(seq)) - - def mission_ack_encode(self, target_system, target_component, type): - ''' - Ack message during MISSION handling. The type field states if this - message is a positive ack (type=0) or if an error - happened (type=non-zero). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - type : See MAV_MISSION_RESULT enum (uint8_t) - - ''' - msg = MAVLink_mission_ack_message(target_system, target_component, type) - msg.pack(self) - return msg - - def mission_ack_send(self, target_system, target_component, type): - ''' - Ack message during MISSION handling. The type field states if this - message is a positive ack (type=0) or if an error - happened (type=non-zero). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - type : See MAV_MISSION_RESULT enum (uint8_t) - - ''' - return self.send(self.mission_ack_encode(target_system, target_component, type)) - - def set_gps_global_origin_encode(self, target_system, latitude, longitude, altitude): - ''' - As local MISSIONs exist, the global MISSION reference allows to - transform between the local coordinate frame and the - global (GPS) coordinate frame. This can be necessary - when e.g. in- and outdoor settings are connected and - the MAV should move from in- to outdoor. - - target_system : System ID (uint8_t) - latitude : global position * 1E7 (int32_t) - longitude : global position * 1E7 (int32_t) - altitude : global position * 1000 (int32_t) - - ''' - msg = MAVLink_set_gps_global_origin_message(target_system, latitude, longitude, altitude) - msg.pack(self) - return msg - - def set_gps_global_origin_send(self, target_system, latitude, longitude, altitude): - ''' - As local MISSIONs exist, the global MISSION reference allows to - transform between the local coordinate frame and the - global (GPS) coordinate frame. This can be necessary - when e.g. in- and outdoor settings are connected and - the MAV should move from in- to outdoor. - - target_system : System ID (uint8_t) - latitude : global position * 1E7 (int32_t) - longitude : global position * 1E7 (int32_t) - altitude : global position * 1000 (int32_t) - - ''' - return self.send(self.set_gps_global_origin_encode(target_system, latitude, longitude, altitude)) - - def gps_global_origin_encode(self, latitude, longitude, altitude): - ''' - Once the MAV sets a new GPS-Local correspondence, this message - announces the origin (0,0,0) position - - latitude : Latitude (WGS84), expressed as * 1E7 (int32_t) - longitude : Longitude (WGS84), expressed as * 1E7 (int32_t) - altitude : Altitude(WGS84), expressed as * 1000 (int32_t) - - ''' - msg = MAVLink_gps_global_origin_message(latitude, longitude, altitude) - msg.pack(self) - return msg - - def gps_global_origin_send(self, latitude, longitude, altitude): - ''' - Once the MAV sets a new GPS-Local correspondence, this message - announces the origin (0,0,0) position - - latitude : Latitude (WGS84), expressed as * 1E7 (int32_t) - longitude : Longitude (WGS84), expressed as * 1E7 (int32_t) - altitude : Altitude(WGS84), expressed as * 1000 (int32_t) - - ''' - return self.send(self.gps_global_origin_encode(latitude, longitude, altitude)) - - def set_local_position_setpoint_encode(self, target_system, target_component, coordinate_frame, x, y, z, yaw): - ''' - Set the setpoint for a local position controller. This is the position - in local coordinates the MAV should fly to. This - message is sent by the path/MISSION planner to the - onboard position controller. As some MAVs have a - degree of freedom in yaw (e.g. all - helicopters/quadrotors), the desired yaw angle is part - of the message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU (uint8_t) - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - msg = MAVLink_set_local_position_setpoint_message(target_system, target_component, coordinate_frame, x, y, z, yaw) - msg.pack(self) - return msg - - def set_local_position_setpoint_send(self, target_system, target_component, coordinate_frame, x, y, z, yaw): - ''' - Set the setpoint for a local position controller. This is the position - in local coordinates the MAV should fly to. This - message is sent by the path/MISSION planner to the - onboard position controller. As some MAVs have a - degree of freedom in yaw (e.g. all - helicopters/quadrotors), the desired yaw angle is part - of the message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU (uint8_t) - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - return self.send(self.set_local_position_setpoint_encode(target_system, target_component, coordinate_frame, x, y, z, yaw)) - - def local_position_setpoint_encode(self, coordinate_frame, x, y, z, yaw): - ''' - Transmit the current local setpoint of the controller to other MAVs - (collision avoidance) and to the GCS. - - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU (uint8_t) - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - msg = MAVLink_local_position_setpoint_message(coordinate_frame, x, y, z, yaw) - msg.pack(self) - return msg - - def local_position_setpoint_send(self, coordinate_frame, x, y, z, yaw): - ''' - Transmit the current local setpoint of the controller to other MAVs - (collision avoidance) and to the GCS. - - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU (uint8_t) - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - return self.send(self.local_position_setpoint_encode(coordinate_frame, x, y, z, yaw)) - - def global_position_setpoint_int_encode(self, coordinate_frame, latitude, longitude, altitude, yaw): - ''' - Transmit the current local setpoint of the controller to other MAVs - (collision avoidance) and to the GCS. - - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT (uint8_t) - latitude : WGS84 Latitude position in degrees * 1E7 (int32_t) - longitude : WGS84 Longitude position in degrees * 1E7 (int32_t) - altitude : WGS84 Altitude in meters * 1000 (positive for up) (int32_t) - yaw : Desired yaw angle in degrees * 100 (int16_t) - - ''' - msg = MAVLink_global_position_setpoint_int_message(coordinate_frame, latitude, longitude, altitude, yaw) - msg.pack(self) - return msg - - def global_position_setpoint_int_send(self, coordinate_frame, latitude, longitude, altitude, yaw): - ''' - Transmit the current local setpoint of the controller to other MAVs - (collision avoidance) and to the GCS. - - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT (uint8_t) - latitude : WGS84 Latitude position in degrees * 1E7 (int32_t) - longitude : WGS84 Longitude position in degrees * 1E7 (int32_t) - altitude : WGS84 Altitude in meters * 1000 (positive for up) (int32_t) - yaw : Desired yaw angle in degrees * 100 (int16_t) - - ''' - return self.send(self.global_position_setpoint_int_encode(coordinate_frame, latitude, longitude, altitude, yaw)) - - def set_global_position_setpoint_int_encode(self, coordinate_frame, latitude, longitude, altitude, yaw): - ''' - Set the current global position setpoint. - - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT (uint8_t) - latitude : WGS84 Latitude position in degrees * 1E7 (int32_t) - longitude : WGS84 Longitude position in degrees * 1E7 (int32_t) - altitude : WGS84 Altitude in meters * 1000 (positive for up) (int32_t) - yaw : Desired yaw angle in degrees * 100 (int16_t) - - ''' - msg = MAVLink_set_global_position_setpoint_int_message(coordinate_frame, latitude, longitude, altitude, yaw) - msg.pack(self) - return msg - - def set_global_position_setpoint_int_send(self, coordinate_frame, latitude, longitude, altitude, yaw): - ''' - Set the current global position setpoint. - - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT (uint8_t) - latitude : WGS84 Latitude position in degrees * 1E7 (int32_t) - longitude : WGS84 Longitude position in degrees * 1E7 (int32_t) - altitude : WGS84 Altitude in meters * 1000 (positive for up) (int32_t) - yaw : Desired yaw angle in degrees * 100 (int16_t) - - ''' - return self.send(self.set_global_position_setpoint_int_encode(coordinate_frame, latitude, longitude, altitude, yaw)) - - def safety_set_allowed_area_encode(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Set a safety zone (volume), which is defined by two corners of a cube. - This message can be used to tell the MAV which - setpoints/MISSIONs to accept and which to reject. - Safety areas are often enforced by national or - competition regulations. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - msg = MAVLink_safety_set_allowed_area_message(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z) - msg.pack(self) - return msg - - def safety_set_allowed_area_send(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Set a safety zone (volume), which is defined by two corners of a cube. - This message can be used to tell the MAV which - setpoints/MISSIONs to accept and which to reject. - Safety areas are often enforced by national or - competition regulations. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - return self.send(self.safety_set_allowed_area_encode(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z)) - - def safety_allowed_area_encode(self, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Read out the safety zone the MAV currently assumes. - - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - msg = MAVLink_safety_allowed_area_message(frame, p1x, p1y, p1z, p2x, p2y, p2z) - msg.pack(self) - return msg - - def safety_allowed_area_send(self, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Read out the safety zone the MAV currently assumes. - - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - return self.send(self.safety_allowed_area_encode(frame, p1x, p1y, p1z, p2x, p2y, p2z)) - - def set_roll_pitch_yaw_thrust_encode(self, target_system, target_component, roll, pitch, yaw, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_set_roll_pitch_yaw_thrust_message(target_system, target_component, roll, pitch, yaw, thrust) - msg.pack(self) - return msg - - def set_roll_pitch_yaw_thrust_send(self, target_system, target_component, roll, pitch, yaw, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.set_roll_pitch_yaw_thrust_encode(target_system, target_component, roll, pitch, yaw, thrust)) - - def set_roll_pitch_yaw_speed_thrust_encode(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_set_roll_pitch_yaw_speed_thrust_message(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust) - msg.pack(self) - return msg - - def set_roll_pitch_yaw_speed_thrust_send(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.set_roll_pitch_yaw_speed_thrust_encode(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust)) - - def roll_pitch_yaw_thrust_setpoint_encode(self, time_boot_ms, roll, pitch, yaw, thrust): - ''' - Setpoint in roll, pitch, yaw currently active on the system. - - time_boot_ms : Timestamp in milliseconds since system boot (uint32_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_roll_pitch_yaw_thrust_setpoint_message(time_boot_ms, roll, pitch, yaw, thrust) - msg.pack(self) - return msg - - def roll_pitch_yaw_thrust_setpoint_send(self, time_boot_ms, roll, pitch, yaw, thrust): - ''' - Setpoint in roll, pitch, yaw currently active on the system. - - time_boot_ms : Timestamp in milliseconds since system boot (uint32_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.roll_pitch_yaw_thrust_setpoint_encode(time_boot_ms, roll, pitch, yaw, thrust)) - - def roll_pitch_yaw_speed_thrust_setpoint_encode(self, time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Setpoint in rollspeed, pitchspeed, yawspeed currently active on the - system. - - time_boot_ms : Timestamp in milliseconds since system boot (uint32_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message(time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust) - msg.pack(self) - return msg - - def roll_pitch_yaw_speed_thrust_setpoint_send(self, time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Setpoint in rollspeed, pitchspeed, yawspeed currently active on the - system. - - time_boot_ms : Timestamp in milliseconds since system boot (uint32_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.roll_pitch_yaw_speed_thrust_setpoint_encode(time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust)) - - def nav_controller_output_encode(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error): - ''' - Outputs of the APM navigation controller. The primary use of this - message is to check the response and signs - of the controller before actual flight and to assist - with tuning controller parameters - - nav_roll : Current desired roll in degrees (float) - nav_pitch : Current desired pitch in degrees (float) - nav_bearing : Current desired heading in degrees (int16_t) - target_bearing : Bearing to current MISSION/target in degrees (int16_t) - wp_dist : Distance to active MISSION in meters (uint16_t) - alt_error : Current altitude error in meters (float) - aspd_error : Current airspeed error in meters/second (float) - xtrack_error : Current crosstrack error on x-y plane in meters (float) - - ''' - msg = MAVLink_nav_controller_output_message(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error) - msg.pack(self) - return msg - - def nav_controller_output_send(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error): - ''' - Outputs of the APM navigation controller. The primary use of this - message is to check the response and signs - of the controller before actual flight and to assist - with tuning controller parameters - - nav_roll : Current desired roll in degrees (float) - nav_pitch : Current desired pitch in degrees (float) - nav_bearing : Current desired heading in degrees (int16_t) - target_bearing : Bearing to current MISSION/target in degrees (int16_t) - wp_dist : Distance to active MISSION in meters (uint16_t) - alt_error : Current altitude error in meters (float) - aspd_error : Current airspeed error in meters/second (float) - xtrack_error : Current crosstrack error on x-y plane in meters (float) - - ''' - return self.send(self.nav_controller_output_encode(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error)) - - def state_correction_encode(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr): - ''' - Corrects the systems state by adding an error correction term to the - position and velocity, and by rotating the attitude by - a correction angle. - - xErr : x position error (float) - yErr : y position error (float) - zErr : z position error (float) - rollErr : roll error (radians) (float) - pitchErr : pitch error (radians) (float) - yawErr : yaw error (radians) (float) - vxErr : x velocity (float) - vyErr : y velocity (float) - vzErr : z velocity (float) - - ''' - msg = MAVLink_state_correction_message(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr) - msg.pack(self) - return msg - - def state_correction_send(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr): - ''' - Corrects the systems state by adding an error correction term to the - position and velocity, and by rotating the attitude by - a correction angle. - - xErr : x position error (float) - yErr : y position error (float) - zErr : z position error (float) - rollErr : roll error (radians) (float) - pitchErr : pitch error (radians) (float) - yawErr : yaw error (radians) (float) - vxErr : x velocity (float) - vyErr : y velocity (float) - vzErr : z velocity (float) - - ''' - return self.send(self.state_correction_encode(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr)) - - def request_data_stream_encode(self, target_system, target_component, req_stream_id, req_message_rate, start_stop): - ''' - - - target_system : The target requested to send the message stream. (uint8_t) - target_component : The target requested to send the message stream. (uint8_t) - req_stream_id : The ID of the requested data stream (uint8_t) - req_message_rate : The requested interval between two messages of this type (uint16_t) - start_stop : 1 to start sending, 0 to stop sending. (uint8_t) - - ''' - msg = MAVLink_request_data_stream_message(target_system, target_component, req_stream_id, req_message_rate, start_stop) - msg.pack(self) - return msg - - def request_data_stream_send(self, target_system, target_component, req_stream_id, req_message_rate, start_stop): - ''' - - - target_system : The target requested to send the message stream. (uint8_t) - target_component : The target requested to send the message stream. (uint8_t) - req_stream_id : The ID of the requested data stream (uint8_t) - req_message_rate : The requested interval between two messages of this type (uint16_t) - start_stop : 1 to start sending, 0 to stop sending. (uint8_t) - - ''' - return self.send(self.request_data_stream_encode(target_system, target_component, req_stream_id, req_message_rate, start_stop)) - - def data_stream_encode(self, stream_id, message_rate, on_off): - ''' - - - stream_id : The ID of the requested data stream (uint8_t) - message_rate : The requested interval between two messages of this type (uint16_t) - on_off : 1 stream is enabled, 0 stream is stopped. (uint8_t) - - ''' - msg = MAVLink_data_stream_message(stream_id, message_rate, on_off) - msg.pack(self) - return msg - - def data_stream_send(self, stream_id, message_rate, on_off): - ''' - - - stream_id : The ID of the requested data stream (uint8_t) - message_rate : The requested interval between two messages of this type (uint16_t) - on_off : 1 stream is enabled, 0 stream is stopped. (uint8_t) - - ''' - return self.send(self.data_stream_encode(stream_id, message_rate, on_off)) - - def manual_control_encode(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual): - ''' - - - target : The system to be controlled (uint8_t) - roll : roll (float) - pitch : pitch (float) - yaw : yaw (float) - thrust : thrust (float) - roll_manual : roll control enabled auto:0, manual:1 (uint8_t) - pitch_manual : pitch auto:0, manual:1 (uint8_t) - yaw_manual : yaw auto:0, manual:1 (uint8_t) - thrust_manual : thrust auto:0, manual:1 (uint8_t) - - ''' - msg = MAVLink_manual_control_message(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual) - msg.pack(self) - return msg - - def manual_control_send(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual): - ''' - - - target : The system to be controlled (uint8_t) - roll : roll (float) - pitch : pitch (float) - yaw : yaw (float) - thrust : thrust (float) - roll_manual : roll control enabled auto:0, manual:1 (uint8_t) - pitch_manual : pitch auto:0, manual:1 (uint8_t) - yaw_manual : yaw auto:0, manual:1 (uint8_t) - thrust_manual : thrust auto:0, manual:1 (uint8_t) - - ''' - return self.send(self.manual_control_encode(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual)) - - def rc_channels_override_encode(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw): - ''' - The RAW values of the RC channels sent to the MAV to override info - received from the RC radio. A value of -1 means no - change to that channel. A value of 0 means control of - that channel should be released back to the RC radio. - The standard PPM modulation is as follows: 1000 - microseconds: 0%, 2000 microseconds: 100%. Individual - receivers/transmitters might violate this - specification. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - - ''' - msg = MAVLink_rc_channels_override_message(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw) - msg.pack(self) - return msg - - def rc_channels_override_send(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw): - ''' - The RAW values of the RC channels sent to the MAV to override info - received from the RC radio. A value of -1 means no - change to that channel. A value of 0 means control of - that channel should be released back to the RC radio. - The standard PPM modulation is as follows: 1000 - microseconds: 0%, 2000 microseconds: 100%. Individual - receivers/transmitters might violate this - specification. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - - ''' - return self.send(self.rc_channels_override_encode(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw)) - - def vfr_hud_encode(self, airspeed, groundspeed, heading, throttle, alt, climb): - ''' - Metrics typically displayed on a HUD for fixed wing aircraft - - airspeed : Current airspeed in m/s (float) - groundspeed : Current ground speed in m/s (float) - heading : Current heading in degrees, in compass units (0..360, 0=north) (int16_t) - throttle : Current throttle setting in integer percent, 0 to 100 (uint16_t) - alt : Current altitude (MSL), in meters (float) - climb : Current climb rate in meters/second (float) - - ''' - msg = MAVLink_vfr_hud_message(airspeed, groundspeed, heading, throttle, alt, climb) - msg.pack(self) - return msg - - def vfr_hud_send(self, airspeed, groundspeed, heading, throttle, alt, climb): - ''' - Metrics typically displayed on a HUD for fixed wing aircraft - - airspeed : Current airspeed in m/s (float) - groundspeed : Current ground speed in m/s (float) - heading : Current heading in degrees, in compass units (0..360, 0=north) (int16_t) - throttle : Current throttle setting in integer percent, 0 to 100 (uint16_t) - alt : Current altitude (MSL), in meters (float) - climb : Current climb rate in meters/second (float) - - ''' - return self.send(self.vfr_hud_encode(airspeed, groundspeed, heading, throttle, alt, climb)) - - def command_long_encode(self, target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7): - ''' - Send a command with up to four parameters to the MAV - - target_system : System which should execute the command (uint8_t) - target_component : Component which should execute the command, 0 for all components (uint8_t) - command : Command ID, as defined by MAV_CMD enum. (uint16_t) - confirmation : 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) (uint8_t) - param1 : Parameter 1, as defined by MAV_CMD enum. (float) - param2 : Parameter 2, as defined by MAV_CMD enum. (float) - param3 : Parameter 3, as defined by MAV_CMD enum. (float) - param4 : Parameter 4, as defined by MAV_CMD enum. (float) - param5 : Parameter 5, as defined by MAV_CMD enum. (float) - param6 : Parameter 6, as defined by MAV_CMD enum. (float) - param7 : Parameter 7, as defined by MAV_CMD enum. (float) - - ''' - msg = MAVLink_command_long_message(target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7) - msg.pack(self) - return msg - - def command_long_send(self, target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7): - ''' - Send a command with up to four parameters to the MAV - - target_system : System which should execute the command (uint8_t) - target_component : Component which should execute the command, 0 for all components (uint8_t) - command : Command ID, as defined by MAV_CMD enum. (uint16_t) - confirmation : 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) (uint8_t) - param1 : Parameter 1, as defined by MAV_CMD enum. (float) - param2 : Parameter 2, as defined by MAV_CMD enum. (float) - param3 : Parameter 3, as defined by MAV_CMD enum. (float) - param4 : Parameter 4, as defined by MAV_CMD enum. (float) - param5 : Parameter 5, as defined by MAV_CMD enum. (float) - param6 : Parameter 6, as defined by MAV_CMD enum. (float) - param7 : Parameter 7, as defined by MAV_CMD enum. (float) - - ''' - return self.send(self.command_long_encode(target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7)) - - def command_ack_encode(self, command, result): - ''' - Report status of a command. Includes feedback wether the command was - executed - - command : Command ID, as defined by MAV_CMD enum. (uint16_t) - result : See MAV_RESULT enum (uint8_t) - - ''' - msg = MAVLink_command_ack_message(command, result) - msg.pack(self) - return msg - - def command_ack_send(self, command, result): - ''' - Report status of a command. Includes feedback wether the command was - executed - - command : Command ID, as defined by MAV_CMD enum. (uint16_t) - result : See MAV_RESULT enum (uint8_t) - - ''' - return self.send(self.command_ack_encode(command, result)) - - def hil_state_encode(self, time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc): - ''' - Sent from simulation to autopilot. This packet is useful for high - throughput applications such as - hardware in the loop simulations. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - - ''' - msg = MAVLink_hil_state_message(time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc) - msg.pack(self) - return msg - - def hil_state_send(self, time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc): - ''' - Sent from simulation to autopilot. This packet is useful for high - throughput applications such as - hardware in the loop simulations. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - - ''' - return self.send(self.hil_state_encode(time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc)) - - def hil_controls_encode(self, time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode): - ''' - Sent from autopilot to simulation. Hardware in the loop control - outputs - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll_ailerons : Control output -1 .. 1 (float) - pitch_elevator : Control output -1 .. 1 (float) - yaw_rudder : Control output -1 .. 1 (float) - throttle : Throttle 0 .. 1 (float) - aux1 : Aux 1, -1 .. 1 (float) - aux2 : Aux 2, -1 .. 1 (float) - aux3 : Aux 3, -1 .. 1 (float) - aux4 : Aux 4, -1 .. 1 (float) - mode : System mode (MAV_MODE) (uint8_t) - nav_mode : Navigation mode (MAV_NAV_MODE) (uint8_t) - - ''' - msg = MAVLink_hil_controls_message(time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode) - msg.pack(self) - return msg - - def hil_controls_send(self, time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode): - ''' - Sent from autopilot to simulation. Hardware in the loop control - outputs - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll_ailerons : Control output -1 .. 1 (float) - pitch_elevator : Control output -1 .. 1 (float) - yaw_rudder : Control output -1 .. 1 (float) - throttle : Throttle 0 .. 1 (float) - aux1 : Aux 1, -1 .. 1 (float) - aux2 : Aux 2, -1 .. 1 (float) - aux3 : Aux 3, -1 .. 1 (float) - aux4 : Aux 4, -1 .. 1 (float) - mode : System mode (MAV_MODE) (uint8_t) - nav_mode : Navigation mode (MAV_NAV_MODE) (uint8_t) - - ''' - return self.send(self.hil_controls_encode(time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode)) - - def hil_rc_inputs_raw_encode(self, time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi): - ''' - Sent from simulation to autopilot. The RAW values of the RC channels - received. The standard PPM modulation is as follows: - 1000 microseconds: 0%, 2000 microseconds: 100%. - Individual receivers/transmitters might violate this - specification. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - chan9_raw : RC channel 9 value, in microseconds (uint16_t) - chan10_raw : RC channel 10 value, in microseconds (uint16_t) - chan11_raw : RC channel 11 value, in microseconds (uint16_t) - chan12_raw : RC channel 12 value, in microseconds (uint16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - msg = MAVLink_hil_rc_inputs_raw_message(time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi) - msg.pack(self) - return msg - - def hil_rc_inputs_raw_send(self, time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi): - ''' - Sent from simulation to autopilot. The RAW values of the RC channels - received. The standard PPM modulation is as follows: - 1000 microseconds: 0%, 2000 microseconds: 100%. - Individual receivers/transmitters might violate this - specification. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - chan9_raw : RC channel 9 value, in microseconds (uint16_t) - chan10_raw : RC channel 10 value, in microseconds (uint16_t) - chan11_raw : RC channel 11 value, in microseconds (uint16_t) - chan12_raw : RC channel 12 value, in microseconds (uint16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - return self.send(self.hil_rc_inputs_raw_encode(time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi)) - - def optical_flow_encode(self, time_usec, sensor_id, flow_x, flow_y, quality, ground_distance): - ''' - Optical flow from a flow sensor (e.g. optical mouse sensor) - - time_usec : Timestamp (UNIX) (uint64_t) - sensor_id : Sensor ID (uint8_t) - flow_x : Flow in pixels in x-sensor direction (int16_t) - flow_y : Flow in pixels in y-sensor direction (int16_t) - quality : Optical flow quality / confidence. 0: bad, 255: maximum quality (uint8_t) - ground_distance : Ground distance in meters (float) - - ''' - msg = MAVLink_optical_flow_message(time_usec, sensor_id, flow_x, flow_y, quality, ground_distance) - msg.pack(self) - return msg - - def optical_flow_send(self, time_usec, sensor_id, flow_x, flow_y, quality, ground_distance): - ''' - Optical flow from a flow sensor (e.g. optical mouse sensor) - - time_usec : Timestamp (UNIX) (uint64_t) - sensor_id : Sensor ID (uint8_t) - flow_x : Flow in pixels in x-sensor direction (int16_t) - flow_y : Flow in pixels in y-sensor direction (int16_t) - quality : Optical flow quality / confidence. 0: bad, 255: maximum quality (uint8_t) - ground_distance : Ground distance in meters (float) - - ''' - return self.send(self.optical_flow_encode(time_usec, sensor_id, flow_x, flow_y, quality, ground_distance)) - - def global_vision_position_estimate_encode(self, usec, x, y, z, roll, pitch, yaw): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X position (float) - y : Global Y position (float) - z : Global Z position (float) - roll : Roll angle in rad (float) - pitch : Pitch angle in rad (float) - yaw : Yaw angle in rad (float) - - ''' - msg = MAVLink_global_vision_position_estimate_message(usec, x, y, z, roll, pitch, yaw) - msg.pack(self) - return msg - - def global_vision_position_estimate_send(self, usec, x, y, z, roll, pitch, yaw): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X position (float) - y : Global Y position (float) - z : Global Z position (float) - roll : Roll angle in rad (float) - pitch : Pitch angle in rad (float) - yaw : Yaw angle in rad (float) - - ''' - return self.send(self.global_vision_position_estimate_encode(usec, x, y, z, roll, pitch, yaw)) - - def vision_position_estimate_encode(self, usec, x, y, z, roll, pitch, yaw): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X position (float) - y : Global Y position (float) - z : Global Z position (float) - roll : Roll angle in rad (float) - pitch : Pitch angle in rad (float) - yaw : Yaw angle in rad (float) - - ''' - msg = MAVLink_vision_position_estimate_message(usec, x, y, z, roll, pitch, yaw) - msg.pack(self) - return msg - - def vision_position_estimate_send(self, usec, x, y, z, roll, pitch, yaw): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X position (float) - y : Global Y position (float) - z : Global Z position (float) - roll : Roll angle in rad (float) - pitch : Pitch angle in rad (float) - yaw : Yaw angle in rad (float) - - ''' - return self.send(self.vision_position_estimate_encode(usec, x, y, z, roll, pitch, yaw)) - - def vision_speed_estimate_encode(self, usec, x, y, z): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X speed (float) - y : Global Y speed (float) - z : Global Z speed (float) - - ''' - msg = MAVLink_vision_speed_estimate_message(usec, x, y, z) - msg.pack(self) - return msg - - def vision_speed_estimate_send(self, usec, x, y, z): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X speed (float) - y : Global Y speed (float) - z : Global Z speed (float) - - ''' - return self.send(self.vision_speed_estimate_encode(usec, x, y, z)) - - def vicon_position_estimate_encode(self, usec, x, y, z, roll, pitch, yaw): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X position (float) - y : Global Y position (float) - z : Global Z position (float) - roll : Roll angle in rad (float) - pitch : Pitch angle in rad (float) - yaw : Yaw angle in rad (float) - - ''' - msg = MAVLink_vicon_position_estimate_message(usec, x, y, z, roll, pitch, yaw) - msg.pack(self) - return msg - - def vicon_position_estimate_send(self, usec, x, y, z, roll, pitch, yaw): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X position (float) - y : Global Y position (float) - z : Global Z position (float) - roll : Roll angle in rad (float) - pitch : Pitch angle in rad (float) - yaw : Yaw angle in rad (float) - - ''' - return self.send(self.vicon_position_estimate_encode(usec, x, y, z, roll, pitch, yaw)) - - def memory_vect_encode(self, address, ver, type, value): - ''' - Send raw controller memory. The use of this message is discouraged for - normal packets, but a quite efficient way for testing - new messages and getting experimental debug output. - - address : Starting address of the debug variables (uint16_t) - ver : Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below (uint8_t) - type : Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 (uint8_t) - value : Memory contents at specified address (int8_t) - - ''' - msg = MAVLink_memory_vect_message(address, ver, type, value) - msg.pack(self) - return msg - - def memory_vect_send(self, address, ver, type, value): - ''' - Send raw controller memory. The use of this message is discouraged for - normal packets, but a quite efficient way for testing - new messages and getting experimental debug output. - - address : Starting address of the debug variables (uint16_t) - ver : Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below (uint8_t) - type : Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 (uint8_t) - value : Memory contents at specified address (int8_t) - - ''' - return self.send(self.memory_vect_encode(address, ver, type, value)) - - def debug_vect_encode(self, name, time_usec, x, y, z): - ''' - - - name : Name (char) - time_usec : Timestamp (uint64_t) - x : x (float) - y : y (float) - z : z (float) - - ''' - msg = MAVLink_debug_vect_message(name, time_usec, x, y, z) - msg.pack(self) - return msg - - def debug_vect_send(self, name, time_usec, x, y, z): - ''' - - - name : Name (char) - time_usec : Timestamp (uint64_t) - x : x (float) - y : y (float) - z : z (float) - - ''' - return self.send(self.debug_vect_encode(name, time_usec, x, y, z)) - - def named_value_float_encode(self, time_boot_ms, name, value): - ''' - Send a key-value pair as float. The use of this message is discouraged - for normal packets, but a quite efficient way for - testing new messages and getting experimental debug - output. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - name : Name of the debug variable (char) - value : Floating point value (float) - - ''' - msg = MAVLink_named_value_float_message(time_boot_ms, name, value) - msg.pack(self) - return msg - - def named_value_float_send(self, time_boot_ms, name, value): - ''' - Send a key-value pair as float. The use of this message is discouraged - for normal packets, but a quite efficient way for - testing new messages and getting experimental debug - output. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - name : Name of the debug variable (char) - value : Floating point value (float) - - ''' - return self.send(self.named_value_float_encode(time_boot_ms, name, value)) - - def named_value_int_encode(self, time_boot_ms, name, value): - ''' - Send a key-value pair as integer. The use of this message is - discouraged for normal packets, but a quite efficient - way for testing new messages and getting experimental - debug output. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - name : Name of the debug variable (char) - value : Signed integer value (int32_t) - - ''' - msg = MAVLink_named_value_int_message(time_boot_ms, name, value) - msg.pack(self) - return msg - - def named_value_int_send(self, time_boot_ms, name, value): - ''' - Send a key-value pair as integer. The use of this message is - discouraged for normal packets, but a quite efficient - way for testing new messages and getting experimental - debug output. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - name : Name of the debug variable (char) - value : Signed integer value (int32_t) - - ''' - return self.send(self.named_value_int_encode(time_boot_ms, name, value)) - - def statustext_encode(self, severity, text): - ''' - Status text message. These messages are printed in yellow in the COMM - console of QGroundControl. WARNING: They consume quite - some bandwidth, so use only for important status and - error messages. If implemented wisely, these messages - are buffered on the MCU and sent only at a limited - rate (e.g. 10 Hz). - - severity : Severity of status, 0 = info message, 255 = critical fault (uint8_t) - text : Status text message, without null termination character (char) - - ''' - msg = MAVLink_statustext_message(severity, text) - msg.pack(self) - return msg - - def statustext_send(self, severity, text): - ''' - Status text message. These messages are printed in yellow in the COMM - console of QGroundControl. WARNING: They consume quite - some bandwidth, so use only for important status and - error messages. If implemented wisely, these messages - are buffered on the MCU and sent only at a limited - rate (e.g. 10 Hz). - - severity : Severity of status, 0 = info message, 255 = critical fault (uint8_t) - text : Status text message, without null termination character (char) - - ''' - return self.send(self.statustext_encode(severity, text)) - - def debug_encode(self, time_boot_ms, ind, value): - ''' - Send a debug value. The index is used to discriminate between values. - These values show up in the plot of QGroundControl as - DEBUG N. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - ind : index of debug variable (uint8_t) - value : DEBUG value (float) - - ''' - msg = MAVLink_debug_message(time_boot_ms, ind, value) - msg.pack(self) - return msg - - def debug_send(self, time_boot_ms, ind, value): - ''' - Send a debug value. The index is used to discriminate between values. - These values show up in the plot of QGroundControl as - DEBUG N. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - ind : index of debug variable (uint8_t) - value : DEBUG value (float) - - ''' - return self.send(self.debug_encode(time_boot_ms, ind, value)) - - def extended_message_encode(self, target_system, target_component, protocol_flags): - ''' - Extended message spacer. - - target_system : System which should execute the command (uint8_t) - target_component : Component which should execute the command, 0 for all components (uint8_t) - protocol_flags : Retransmission / ACK flags (uint8_t) - - ''' - msg = MAVLink_extended_message_message(target_system, target_component, protocol_flags) - msg.pack(self) - return msg - - def extended_message_send(self, target_system, target_component, protocol_flags): - ''' - Extended message spacer. - - target_system : System which should execute the command (uint8_t) - target_component : Component which should execute the command, 0 for all components (uint8_t) - protocol_flags : Retransmission / ACK flags (uint8_t) - - ''' - return self.send(self.extended_message_encode(target_system, target_component, protocol_flags)) - diff --git a/mavlink/share/pyshared/pymavlink/mavutil.py b/mavlink/share/pyshared/pymavlink/mavutil.py deleted file mode 100644 index c0f625214..000000000 --- a/mavlink/share/pyshared/pymavlink/mavutil.py +++ /dev/null @@ -1,678 +0,0 @@ -#!/usr/bin/env python -''' -mavlink python utility functions - -Copyright Andrew Tridgell 2011 -Released under GNU GPL version 3 or later -''' - -import socket, math, struct, time, os, fnmatch, array, sys, errno -from math import * -from mavextra import * - -if os.getenv('MAVLINK10'): - import mavlinkv10 as mavlink -else: - import mavlink - -def evaluate_expression(expression, vars): - '''evaluation an expression''' - try: - v = eval(expression, globals(), vars) - except NameError: - return None - return v - -def evaluate_condition(condition, vars): - '''evaluation a conditional (boolean) statement''' - if condition is None: - return True - v = evaluate_expression(condition, vars) - if v is None: - return False - return v - - -class mavfile(object): - '''a generic mavlink port''' - def __init__(self, fd, address, source_system=255, notimestamps=False): - self.fd = fd - self.address = address - self.messages = { 'MAV' : self } - if mavlink.WIRE_PROTOCOL_VERSION == "1.0": - self.messages['HOME'] = mavlink.MAVLink_gps_raw_int_message(0,0,0,0,0,0,0,0,0,0) - mavlink.MAVLink_waypoint_message = mavlink.MAVLink_mission_item_message - else: - self.messages['HOME'] = mavlink.MAVLink_gps_raw_message(0,0,0,0,0,0,0,0,0) - self.params = {} - self.mav = None - self.target_system = 0 - self.target_component = 0 - self.mav = mavlink.MAVLink(self, srcSystem=source_system) - self.mav.robust_parsing = True - self.logfile = None - self.logfile_raw = None - self.param_fetch_in_progress = False - self.param_fetch_complete = False - self.start_time = time.time() - self.flightmode = "UNKNOWN" - self.timestamp = 0 - self.message_hooks = [] - self.idle_hooks = [] - self.usec = 0 - self.notimestamps = notimestamps - self._timestamp = None - - def recv(self, n=None): - '''default recv method''' - raise RuntimeError('no recv() method supplied') - - def close(self, n=None): - '''default close method''' - raise RuntimeError('no close() method supplied') - - def write(self, buf): - '''default write method''' - raise RuntimeError('no write() method supplied') - - def pre_message(self): - '''default pre message call''' - return - - def post_message(self, msg): - '''default post message call''' - msg._timestamp = time.time() - type = msg.get_type() - self.messages[type] = msg - - if self._timestamp is not None: - if self.notimestamps: - if 'usec' in msg.__dict__: - self.usec = msg.usec / 1.0e6 - msg._timestamp = self.usec - else: - msg._timestamp = self._timestamp - - self.timestamp = msg._timestamp - if type == 'HEARTBEAT': - self.target_system = msg.get_srcSystem() - self.target_component = msg.get_srcComponent() - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': - self.flightmode = mode_string_v10(msg) - elif type == 'PARAM_VALUE': - self.params[str(msg.param_id)] = msg.param_value - if msg.param_index+1 == msg.param_count: - self.param_fetch_in_progress = False - self.param_fetch_complete = True - elif type == 'SYS_STATUS' and mavlink.WIRE_PROTOCOL_VERSION == '0.9': - self.flightmode = mode_string_v09(msg) - elif type == 'GPS_RAW': - if self.messages['HOME'].fix_type < 2: - self.messages['HOME'] = msg - for hook in self.message_hooks: - hook(self, msg) - - - def recv_msg(self): - '''message receive routine''' - self.pre_message() - while True: - n = self.mav.bytes_needed() - s = self.recv(n) - if len(s) == 0 and len(self.mav.buf) == 0: - return None - if self.logfile_raw: - self.logfile_raw.write(str(s)) - msg = self.mav.parse_char(s) - if msg: - self.post_message(msg) - return msg - - def recv_match(self, condition=None, type=None, blocking=False): - '''recv the next MAVLink message that matches the given condition''' - while True: - m = self.recv_msg() - if m is None: - if blocking: - for hook in self.idle_hooks: - hook(self) - time.sleep(0.01) - continue - return None - if type is not None and type != m.get_type(): - continue - if not evaluate_condition(condition, self.messages): - continue - return m - - def setup_logfile(self, logfile, mode='w'): - '''start logging to the given logfile, with timestamps''' - self.logfile = open(logfile, mode=mode) - - def setup_logfile_raw(self, logfile, mode='w'): - '''start logging raw bytes to the given logfile, without timestamps''' - self.logfile_raw = open(logfile, mode=mode) - - def wait_heartbeat(self, blocking=True): - '''wait for a heartbeat so we know the target system IDs''' - return self.recv_match(type='HEARTBEAT', blocking=blocking) - - def param_fetch_all(self): - '''initiate fetch of all parameters''' - if time.time() - getattr(self, 'param_fetch_start', 0) < 2.0: - # don't fetch too often - return - self.param_fetch_start = time.time() - self.param_fetch_in_progress = True - self.mav.param_request_list_send(self.target_system, self.target_component) - - def time_since(self, mtype): - '''return the time since the last message of type mtype was received''' - if not mtype in self.messages: - return time.time() - self.start_time - return time.time() - self.messages[mtype]._timestamp - - def param_set_send(self, parm_name, parm_value, parm_type=None): - '''wrapper for parameter set''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': - if parm_type == None: - parm_type = mavlink.MAV_VAR_FLOAT - self.mav.param_set_send(self.target_system, self.target_component, - parm_name, parm_value, parm_type) - else: - self.mav.param_set_send(self.target_system, self.target_component, - parm_name, parm_value) - - def waypoint_request_list_send(self): - '''wrapper for waypoint_request_list_send''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': - self.mav.mission_request_list_send(self.target_system, self.target_component) - else: - self.mav.waypoint_request_list_send(self.target_system, self.target_component) - - def waypoint_clear_all_send(self): - '''wrapper for waypoint_clear_all_send''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': - self.mav.mission_clear_all_send(self.target_system, self.target_component) - else: - self.mav.waypoint_clear_all_send(self.target_system, self.target_component) - - def waypoint_request_send(self, seq): - '''wrapper for waypoint_request_send''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': - self.mav.mission_request_send(self.target_system, self.target_component, seq) - else: - self.mav.waypoint_request_send(self.target_system, self.target_component, seq) - - def waypoint_set_current_send(self, seq): - '''wrapper for waypoint_set_current_send''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': - self.mav.mission_set_current_send(self.target_system, self.target_component, seq) - else: - self.mav.waypoint_set_current_send(self.target_system, self.target_component, seq) - - def waypoint_count_send(self, seq): - '''wrapper for waypoint_count_send''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': - self.mav.mission_count_send(self.target_system, self.target_component, seq) - else: - self.mav.waypoint_count_send(self.target_system, self.target_component, seq) - - -class mavserial(mavfile): - '''a serial mavlink port''' - def __init__(self, device, baud=115200, autoreconnect=False, source_system=255): - import serial - self.baud = baud - self.device = device - self.autoreconnect = autoreconnect - self.port = serial.Serial(self.device, self.baud, timeout=0, - dsrdtr=False, rtscts=False, xonxoff=False) - - try: - fd = self.port.fileno() - except Exception: - fd = None - mavfile.__init__(self, fd, device, source_system=source_system) - - def close(self): - self.port.close() - - def recv(self,n=None): - if n is None: - n = self.mav.bytes_needed() - if self.fd is None: - waiting = self.port.inWaiting() - if waiting < n: - n = waiting - return self.port.read(n) - - def write(self, buf): - try: - return self.port.write(buf) - except OSError: - if self.autoreconnect: - self.reset() - return -1 - - def reset(self): - import serial - self.port.close() - while True: - try: - self.port = serial.Serial(self.device, self.baud, timeout=1, - dsrdtr=False, rtscts=False, xonxoff=False) - try: - self.fd = self.port.fileno() - except Exception: - self.fd = None - return - except Exception: - print("Failed to reopen %s" % self.device) - time.sleep(1) - - -class mavudp(mavfile): - '''a UDP mavlink socket''' - def __init__(self, device, input=True, source_system=255): - a = device.split(':') - if len(a) != 2: - print("UDP ports must be specified as host:port") - sys.exit(1) - self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - self.udp_server = input - if input: - self.port.bind((a[0], int(a[1]))) - else: - self.destination_addr = (a[0], int(a[1])) - self.port.setblocking(0) - self.last_address = None - mavfile.__init__(self, self.port.fileno(), device, source_system=source_system) - - def close(self): - self.port.close() - - def recv(self,n=None): - try: - data, self.last_address = self.port.recvfrom(300) - except socket.error as e: - if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]: - return "" - raise - return data - - def write(self, buf): - try: - if self.udp_server: - if self.last_address: - self.port.sendto(buf, self.last_address) - else: - self.port.sendto(buf, self.destination_addr) - except socket.error: - pass - - def recv_msg(self): - '''message receive routine for UDP link''' - self.pre_message() - s = self.recv() - if len(s) == 0: - return None - msg = self.mav.parse_buffer(s) - if msg is not None: - for m in msg: - self.post_message(m) - return msg[0] - return None - - -class mavtcp(mavfile): - '''a TCP mavlink socket''' - def __init__(self, device, source_system=255): - a = device.split(':') - if len(a) != 2: - print("TCP ports must be specified as host:port") - sys.exit(1) - self.port = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self.destination_addr = (a[0], int(a[1])) - self.port.connect(self.destination_addr) - self.port.setblocking(0) - self.port.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1) - mavfile.__init__(self, self.port.fileno(), device, source_system=source_system) - - def close(self): - self.port.close() - - def recv(self,n=None): - if n is None: - n = self.mav.bytes_needed() - try: - data = self.port.recv(n) - except socket.error as e: - if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]: - return "" - raise - return data - - def write(self, buf): - try: - self.port.send(buf) - except socket.error: - pass - - -class mavlogfile(mavfile): - '''a MAVLink logfile reader/writer''' - def __init__(self, filename, planner_format=None, - write=False, append=False, - robust_parsing=True, notimestamps=False, source_system=255): - self.filename = filename - self.writeable = write - self.robust_parsing = robust_parsing - self.planner_format = planner_format - self._two64 = math.pow(2.0, 63) - mode = 'rb' - if self.writeable: - if append: - mode = 'ab' - else: - mode = 'wb' - self.f = open(filename, mode) - self.filesize = os.path.getsize(filename) - self.percent = 0 - mavfile.__init__(self, None, filename, source_system=source_system, notimestamps=notimestamps) - if self.notimestamps: - self._timestamp = 0 - else: - self._timestamp = time.time() - - def close(self): - self.f.close() - - def recv(self,n=None): - if n is None: - n = self.mav.bytes_needed() - return self.f.read(n) - - def write(self, buf): - self.f.write(buf) - - def pre_message(self): - '''read timestamp if needed''' - # read the timestamp - self.percent = (100.0 * self.f.tell()) / self.filesize - if self.notimestamps: - return - if self.planner_format: - tbuf = self.f.read(21) - if len(tbuf) != 21 or tbuf[0] != '-' or tbuf[20] != ':': - raise RuntimeError('bad planner timestamp %s' % tbuf) - hnsec = self._two64 + float(tbuf[0:20]) - t = hnsec * 1.0e-7 # convert to seconds - t -= 719163 * 24 * 60 * 60 # convert to 1970 base - else: - tbuf = self.f.read(8) - if len(tbuf) != 8: - return - (tusec,) = struct.unpack('>Q', tbuf) - t = tusec * 1.0e-6 - self._timestamp = t - - def post_message(self, msg): - '''add timestamp to message''' - # read the timestamp - super(mavlogfile, self).post_message(msg) - if self.planner_format: - self.f.read(1) # trailing newline - self.timestamp = msg._timestamp - -class mavchildexec(mavfile): - '''a MAVLink child processes reader/writer''' - def __init__(self, filename, source_system=255): - from subprocess import Popen, PIPE - import fcntl - - self.filename = filename - self.child = Popen(filename, shell=True, stdout=PIPE, stdin=PIPE) - self.fd = self.child.stdout.fileno() - - fl = fcntl.fcntl(self.fd, fcntl.F_GETFL) - fcntl.fcntl(self.fd, fcntl.F_SETFL, fl | os.O_NONBLOCK) - - fl = fcntl.fcntl(self.child.stdout.fileno(), fcntl.F_GETFL) - fcntl.fcntl(self.child.stdout.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK) - - mavfile.__init__(self, self.fd, filename, source_system=source_system) - - def close(self): - self.child.close() - - def recv(self,n=None): - try: - x = self.child.stdout.read(1) - except Exception: - return '' - return x - - def write(self, buf): - self.child.stdin.write(buf) - - -def mavlink_connection(device, baud=115200, source_system=255, - planner_format=None, write=False, append=False, - robust_parsing=True, notimestamps=False, input=True): - '''make a serial or UDP mavlink connection''' - if device.startswith('tcp:'): - return mavtcp(device[4:], source_system=source_system) - if device.startswith('udp:'): - return mavudp(device[4:], input=input, source_system=source_system) - if device.find(':') != -1 and not device.endswith('log'): - return mavudp(device, source_system=source_system, input=input) - if os.path.isfile(device): - if device.endswith(".elf"): - return mavchildexec(device, source_system=source_system) - else: - return mavlogfile(device, planner_format=planner_format, write=write, - append=append, robust_parsing=robust_parsing, notimestamps=notimestamps, - source_system=source_system) - return mavserial(device, baud=baud, source_system=source_system) - -class periodic_event(object): - '''a class for fixed frequency events''' - def __init__(self, frequency): - self.frequency = float(frequency) - self.last_time = time.time() - - def force(self): - '''force immediate triggering''' - self.last_time = 0 - - def trigger(self): - '''return True if we should trigger now''' - tnow = time.time() - if self.last_time + (1.0/self.frequency) <= tnow: - self.last_time = tnow - return True - return False - - -try: - from curses import ascii - have_ascii = True -except: - have_ascii = False - -def is_printable(c): - '''see if a character is printable''' - global have_ascii - if have_ascii: - return ascii.isprint(c) - if isinstance(c, int): - ic = c - else: - ic = ord(c) - return ic >= 32 and ic <= 126 - -def all_printable(buf): - '''see if a string is all printable''' - for c in buf: - if not is_printable(c) and not c in ['\r', '\n', '\t']: - return False - return True - -class SerialPort(object): - '''auto-detected serial port''' - def __init__(self, device, description=None, hwid=None): - self.device = device - self.description = description - self.hwid = hwid - - def __str__(self): - ret = self.device - if self.description is not None: - ret += " : " + self.description - if self.hwid is not None: - ret += " : " + self.hwid - return ret - -def auto_detect_serial_win32(preferred_list=['*']): - '''try to auto-detect serial ports on win32''' - try: - import scanwin32 - list = sorted(scanwin32.comports()) - except: - return [] - ret = [] - for order, port, desc, hwid in list: - for preferred in preferred_list: - if fnmatch.fnmatch(desc, preferred) or fnmatch.fnmatch(hwid, preferred): - ret.append(SerialPort(port, description=desc, hwid=hwid)) - if len(ret) > 0: - return ret - # now the rest - for order, port, desc, hwid in list: - ret.append(SerialPort(port, description=desc, hwid=hwid)) - return ret - - - - -def auto_detect_serial_unix(preferred_list=['*']): - '''try to auto-detect serial ports on win32''' - import glob - glist = glob.glob('/dev/ttyS*') + glob.glob('/dev/ttyUSB*') + glob.glob('/dev/ttyACM*') + glob.glob('/dev/serial/by-id/*') - ret = [] - # try preferred ones first - for d in glist: - for preferred in preferred_list: - if fnmatch.fnmatch(d, preferred): - ret.append(SerialPort(d)) - if len(ret) > 0: - return ret - # now the rest - for d in glist: - ret.append(SerialPort(d)) - return ret - - - -def auto_detect_serial(preferred_list=['*']): - '''try to auto-detect serial port''' - # see if - if os.name == 'nt': - return auto_detect_serial_win32(preferred_list=preferred_list) - return auto_detect_serial_unix(preferred_list=preferred_list) - -def mode_string_v09(msg): - '''mode string for 0.9 protocol''' - mode = msg.mode - nav_mode = msg.nav_mode - - MAV_MODE_UNINIT = 0 - MAV_MODE_MANUAL = 2 - MAV_MODE_GUIDED = 3 - MAV_MODE_AUTO = 4 - MAV_MODE_TEST1 = 5 - MAV_MODE_TEST2 = 6 - MAV_MODE_TEST3 = 7 - - MAV_NAV_GROUNDED = 0 - MAV_NAV_LIFTOFF = 1 - MAV_NAV_HOLD = 2 - MAV_NAV_WAYPOINT = 3 - MAV_NAV_VECTOR = 4 - MAV_NAV_RETURNING = 5 - MAV_NAV_LANDING = 6 - MAV_NAV_LOST = 7 - MAV_NAV_LOITER = 8 - - cmode = (mode, nav_mode) - mapping = { - (MAV_MODE_UNINIT, MAV_NAV_GROUNDED) : "INITIALISING", - (MAV_MODE_MANUAL, MAV_NAV_VECTOR) : "MANUAL", - (MAV_MODE_TEST3, MAV_NAV_VECTOR) : "CIRCLE", - (MAV_MODE_GUIDED, MAV_NAV_VECTOR) : "GUIDED", - (MAV_MODE_TEST1, MAV_NAV_VECTOR) : "STABILIZE", - (MAV_MODE_TEST2, MAV_NAV_LIFTOFF) : "FBWA", - (MAV_MODE_AUTO, MAV_NAV_WAYPOINT) : "AUTO", - (MAV_MODE_AUTO, MAV_NAV_RETURNING) : "RTL", - (MAV_MODE_AUTO, MAV_NAV_LOITER) : "LOITER", - (MAV_MODE_AUTO, MAV_NAV_LIFTOFF) : "TAKEOFF", - (MAV_MODE_AUTO, MAV_NAV_LANDING) : "LANDING", - (MAV_MODE_AUTO, MAV_NAV_HOLD) : "LOITER", - (MAV_MODE_GUIDED, MAV_NAV_VECTOR) : "GUIDED", - (MAV_MODE_GUIDED, MAV_NAV_WAYPOINT) : "GUIDED", - (100, MAV_NAV_VECTOR) : "STABILIZE", - (101, MAV_NAV_VECTOR) : "ACRO", - (102, MAV_NAV_VECTOR) : "ALT_HOLD", - (107, MAV_NAV_VECTOR) : "CIRCLE", - (109, MAV_NAV_VECTOR) : "LAND", - } - if cmode in mapping: - return mapping[cmode] - return "Mode(%s,%s)" % cmode - -def mode_string_v10(msg): - '''mode string for 1.0 protocol, from heartbeat''' - if not msg.base_mode & mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED: - return "Mode(0x%08x)" % msg.base_mode - mapping = { - 0 : 'MANUAL', - 1 : 'CIRCLE', - 2 : 'STABILIZE', - 5 : 'FBWA', - 6 : 'FBWB', - 7 : 'FBWC', - 10 : 'AUTO', - 11 : 'RTL', - 12 : 'LOITER', - 13 : 'TAKEOFF', - 14 : 'LAND', - 15 : 'GUIDED', - 16 : 'INITIALISING' - } - if msg.custom_mode in mapping: - return mapping[msg.custom_mode] - return "Mode(%u)" % msg.custom_mode - - - -class x25crc(object): - '''x25 CRC - based on checksum.h from mavlink library''' - def __init__(self, buf=''): - self.crc = 0xffff - self.accumulate(buf) - - def accumulate(self, buf): - '''add in some more bytes''' - bytes = array.array('B') - if isinstance(buf, array.array): - bytes.extend(buf) - else: - bytes.fromstring(buf) - accum = self.crc - for b in bytes: - tmp = b ^ (accum & 0xff) - tmp = (tmp ^ (tmp<<4)) & 0xFF - accum = (accum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4) - accum = accum & 0xFFFF - self.crc = accum diff --git a/mavlink/share/pyshared/pymavlink/mavwp.py b/mavlink/share/pyshared/pymavlink/mavwp.py deleted file mode 100644 index 6fd1e10e2..000000000 --- a/mavlink/share/pyshared/pymavlink/mavwp.py +++ /dev/null @@ -1,200 +0,0 @@ -''' -module for loading/saving waypoints -''' - -import os - -if os.getenv('MAVLINK10'): - import mavlinkv10 as mavlink -else: - import mavlink - -class MAVWPError(Exception): - '''MAVLink WP error class''' - def __init__(self, msg): - Exception.__init__(self, msg) - self.message = msg - -class MAVWPLoader(object): - '''MAVLink waypoint loader''' - def __init__(self, target_system=0, target_component=0): - self.wpoints = [] - self.target_system = target_system - self.target_component = target_component - - - def count(self): - '''return number of waypoints''' - return len(self.wpoints) - - def wp(self, i): - '''return a waypoint''' - return self.wpoints[i] - - def add(self, w): - '''add a waypoint''' - w.seq = self.count() - self.wpoints.append(w) - - def remove(self, w): - '''remove a waypoint''' - self.wpoints.remove(w) - - def clear(self): - '''clear waypoint list''' - self.wpoints = [] - - def _read_waypoint_v100(self, line): - '''read a version 100 waypoint''' - cmdmap = { - 2 : mavlink.MAV_CMD_NAV_TAKEOFF, - 3 : mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, - 4 : mavlink.MAV_CMD_NAV_LAND, - 24: mavlink.MAV_CMD_NAV_TAKEOFF, - 26: mavlink.MAV_CMD_NAV_LAND, - 25: mavlink.MAV_CMD_NAV_WAYPOINT , - 27: mavlink.MAV_CMD_NAV_LOITER_UNLIM - } - a = line.split() - if len(a) != 13: - raise MAVWPError("invalid waypoint line with %u values" % len(a)) - w = mavlink.MAVLink_waypoint_message(self.target_system, self.target_component, - int(a[0]), # seq - int(a[1]), # frame - int(a[2]), # action - int(a[7]), # current - int(a[12]), # autocontinue - float(a[5]), # param1, - float(a[6]), # param2, - float(a[3]), # param3 - float(a[4]), # param4 - float(a[9]), # x, latitude - float(a[8]), # y, longitude - float(a[10]) # z - ) - if not w.command in cmdmap: - raise MAVWPError("Unknown v100 waypoint action %u" % w.command) - - w.command = cmdmap[w.command] - return w - - def _read_waypoint_v110(self, line): - '''read a version 110 waypoint''' - a = line.split() - if len(a) != 12: - raise MAVWPError("invalid waypoint line with %u values" % len(a)) - w = mavlink.MAVLink_waypoint_message(self.target_system, self.target_component, - int(a[0]), # seq - int(a[2]), # frame - int(a[3]), # command - int(a[1]), # current - int(a[11]), # autocontinue - float(a[4]), # param1, - float(a[5]), # param2, - float(a[6]), # param3 - float(a[7]), # param4 - float(a[8]), # x (latitude) - float(a[9]), # y (longitude) - float(a[10]) # z (altitude) - ) - return w - - - def load(self, filename): - '''load waypoints from a file. - returns number of waypoints loaded''' - f = open(filename, mode='r') - version_line = f.readline().strip() - if version_line == "QGC WPL 100": - readfn = self._read_waypoint_v100 - elif version_line == "QGC WPL 110": - readfn = self._read_waypoint_v110 - else: - f.close() - raise MAVWPError("Unsupported waypoint format '%s'" % version_line) - - self.clear() - - for line in f: - if line.startswith('#'): - continue - line = line.strip() - if not line: - continue - w = readfn(line) - if w is not None: - self.add(w) - f.close() - return len(self.wpoints) - - - def save(self, filename): - '''save waypoints to a file''' - f = open(filename, mode='w') - f.write("QGC WPL 110\n") - for w in self.wpoints: - f.write("%u\t%u\t%u\t%u\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%u\n" % ( - w.seq, w.current, w.frame, w.command, - w.param1, w.param2, w.param3, w.param4, - w.x, w.y, w.z, w.autocontinue)) - f.close() - - -class MAVFenceError(Exception): - '''MAVLink fence error class''' - def __init__(self, msg): - Exception.__init__(self, msg) - self.message = msg - -class MAVFenceLoader(object): - '''MAVLink geo-fence loader''' - def __init__(self, target_system=0, target_component=0): - self.points = [] - self.target_system = target_system - self.target_component = target_component - - def count(self): - '''return number of points''' - return len(self.points) - - def point(self, i): - '''return a point''' - return self.points[i] - - def add(self, p): - '''add a point''' - self.points.append(p) - - def clear(self): - '''clear point list''' - self.points = [] - - def load(self, filename): - '''load points from a file. - returns number of points loaded''' - f = open(filename, mode='r') - self.clear() - for line in f: - if line.startswith('#'): - continue - line = line.strip() - if not line: - continue - a = line.split() - if len(a) != 2: - raise MAVFenceError("invalid fence point line: %s" % line) - p = mavlink.MAVLink_fence_point_message(self.target_system, self.target_component, - self.count(), 0, float(a[0]), float(a[1])) - self.add(p) - f.close() - for i in range(self.count()): - self.points[i].count = self.count() - return len(self.points) - - - def save(self, filename): - '''save fence points to a file''' - f = open(filename, mode='w') - for p in self.points: - f.write("%f\t%f\n" % (p.lat, p.lng)) - f.close() diff --git a/mavlink/share/pyshared/pymavlink/scanwin32.py b/mavlink/share/pyshared/pymavlink/scanwin32.py deleted file mode 100644 index f90ed5f4d..000000000 --- a/mavlink/share/pyshared/pymavlink/scanwin32.py +++ /dev/null @@ -1,236 +0,0 @@ -#!/usr/bin/env python - -# this is taken from the pySerial documentation at -# http://pyserial.sourceforge.net/examples.html -import ctypes -import re - -def ValidHandle(value): - if value == 0: - raise ctypes.WinError() - return value - -NULL = 0 -HDEVINFO = ctypes.c_int -BOOL = ctypes.c_int -CHAR = ctypes.c_char -PCTSTR = ctypes.c_char_p -HWND = ctypes.c_uint -DWORD = ctypes.c_ulong -PDWORD = ctypes.POINTER(DWORD) -ULONG = ctypes.c_ulong -ULONG_PTR = ctypes.POINTER(ULONG) -#~ PBYTE = ctypes.c_char_p -PBYTE = ctypes.c_void_p - -class GUID(ctypes.Structure): - _fields_ = [ - ('Data1', ctypes.c_ulong), - ('Data2', ctypes.c_ushort), - ('Data3', ctypes.c_ushort), - ('Data4', ctypes.c_ubyte*8), - ] - def __str__(self): - return "{%08x-%04x-%04x-%s-%s}" % ( - self.Data1, - self.Data2, - self.Data3, - ''.join(["%02x" % d for d in self.Data4[:2]]), - ''.join(["%02x" % d for d in self.Data4[2:]]), - ) - -class SP_DEVINFO_DATA(ctypes.Structure): - _fields_ = [ - ('cbSize', DWORD), - ('ClassGuid', GUID), - ('DevInst', DWORD), - ('Reserved', ULONG_PTR), - ] - def __str__(self): - return "ClassGuid:%s DevInst:%s" % (self.ClassGuid, self.DevInst) -PSP_DEVINFO_DATA = ctypes.POINTER(SP_DEVINFO_DATA) - -class SP_DEVICE_INTERFACE_DATA(ctypes.Structure): - _fields_ = [ - ('cbSize', DWORD), - ('InterfaceClassGuid', GUID), - ('Flags', DWORD), - ('Reserved', ULONG_PTR), - ] - def __str__(self): - return "InterfaceClassGuid:%s Flags:%s" % (self.InterfaceClassGuid, self.Flags) - -PSP_DEVICE_INTERFACE_DATA = ctypes.POINTER(SP_DEVICE_INTERFACE_DATA) - -PSP_DEVICE_INTERFACE_DETAIL_DATA = ctypes.c_void_p - -class dummy(ctypes.Structure): - _fields_=[("d1", DWORD), ("d2", CHAR)] - _pack_ = 1 -SIZEOF_SP_DEVICE_INTERFACE_DETAIL_DATA_A = ctypes.sizeof(dummy) - -SetupDiDestroyDeviceInfoList = ctypes.windll.setupapi.SetupDiDestroyDeviceInfoList -SetupDiDestroyDeviceInfoList.argtypes = [HDEVINFO] -SetupDiDestroyDeviceInfoList.restype = BOOL - -SetupDiGetClassDevs = ctypes.windll.setupapi.SetupDiGetClassDevsA -SetupDiGetClassDevs.argtypes = [ctypes.POINTER(GUID), PCTSTR, HWND, DWORD] -SetupDiGetClassDevs.restype = ValidHandle # HDEVINFO - -SetupDiEnumDeviceInterfaces = ctypes.windll.setupapi.SetupDiEnumDeviceInterfaces -SetupDiEnumDeviceInterfaces.argtypes = [HDEVINFO, PSP_DEVINFO_DATA, ctypes.POINTER(GUID), DWORD, PSP_DEVICE_INTERFACE_DATA] -SetupDiEnumDeviceInterfaces.restype = BOOL - -SetupDiGetDeviceInterfaceDetail = ctypes.windll.setupapi.SetupDiGetDeviceInterfaceDetailA -SetupDiGetDeviceInterfaceDetail.argtypes = [HDEVINFO, PSP_DEVICE_INTERFACE_DATA, PSP_DEVICE_INTERFACE_DETAIL_DATA, DWORD, PDWORD, PSP_DEVINFO_DATA] -SetupDiGetDeviceInterfaceDetail.restype = BOOL - -SetupDiGetDeviceRegistryProperty = ctypes.windll.setupapi.SetupDiGetDeviceRegistryPropertyA -SetupDiGetDeviceRegistryProperty.argtypes = [HDEVINFO, PSP_DEVINFO_DATA, DWORD, PDWORD, PBYTE, DWORD, PDWORD] -SetupDiGetDeviceRegistryProperty.restype = BOOL - - -GUID_CLASS_COMPORT = GUID(0x86e0d1e0L, 0x8089, 0x11d0, - (ctypes.c_ubyte*8)(0x9c, 0xe4, 0x08, 0x00, 0x3e, 0x30, 0x1f, 0x73)) - -DIGCF_PRESENT = 2 -DIGCF_DEVICEINTERFACE = 16 -INVALID_HANDLE_VALUE = 0 -ERROR_INSUFFICIENT_BUFFER = 122 -SPDRP_HARDWAREID = 1 -SPDRP_FRIENDLYNAME = 12 -SPDRP_LOCATION_INFORMATION = 13 -ERROR_NO_MORE_ITEMS = 259 - -def comports(available_only=True): - """This generator scans the device registry for com ports and yields - (order, port, desc, hwid). If available_only is true only return currently - existing ports. Order is a helper to get sorted lists. it can be ignored - otherwise.""" - flags = DIGCF_DEVICEINTERFACE - if available_only: - flags |= DIGCF_PRESENT - g_hdi = SetupDiGetClassDevs(ctypes.byref(GUID_CLASS_COMPORT), None, NULL, flags); - #~ for i in range(256): - for dwIndex in range(256): - did = SP_DEVICE_INTERFACE_DATA() - did.cbSize = ctypes.sizeof(did) - - if not SetupDiEnumDeviceInterfaces( - g_hdi, - None, - ctypes.byref(GUID_CLASS_COMPORT), - dwIndex, - ctypes.byref(did) - ): - if ctypes.GetLastError() != ERROR_NO_MORE_ITEMS: - raise ctypes.WinError() - break - - dwNeeded = DWORD() - # get the size - if not SetupDiGetDeviceInterfaceDetail( - g_hdi, - ctypes.byref(did), - None, 0, ctypes.byref(dwNeeded), - None - ): - # Ignore ERROR_INSUFFICIENT_BUFFER - if ctypes.GetLastError() != ERROR_INSUFFICIENT_BUFFER: - raise ctypes.WinError() - # allocate buffer - class SP_DEVICE_INTERFACE_DETAIL_DATA_A(ctypes.Structure): - _fields_ = [ - ('cbSize', DWORD), - ('DevicePath', CHAR*(dwNeeded.value - ctypes.sizeof(DWORD))), - ] - def __str__(self): - return "DevicePath:%s" % (self.DevicePath,) - idd = SP_DEVICE_INTERFACE_DETAIL_DATA_A() - idd.cbSize = SIZEOF_SP_DEVICE_INTERFACE_DETAIL_DATA_A - devinfo = SP_DEVINFO_DATA() - devinfo.cbSize = ctypes.sizeof(devinfo) - if not SetupDiGetDeviceInterfaceDetail( - g_hdi, - ctypes.byref(did), - ctypes.byref(idd), dwNeeded, None, - ctypes.byref(devinfo) - ): - raise ctypes.WinError() - - # hardware ID - szHardwareID = ctypes.create_string_buffer(250) - if not SetupDiGetDeviceRegistryProperty( - g_hdi, - ctypes.byref(devinfo), - SPDRP_HARDWAREID, - None, - ctypes.byref(szHardwareID), ctypes.sizeof(szHardwareID) - 1, - None - ): - # Ignore ERROR_INSUFFICIENT_BUFFER - if ctypes.GetLastError() != ERROR_INSUFFICIENT_BUFFER: - raise ctypes.WinError() - - # friendly name - szFriendlyName = ctypes.create_string_buffer(1024) - if not SetupDiGetDeviceRegistryProperty( - g_hdi, - ctypes.byref(devinfo), - SPDRP_FRIENDLYNAME, - None, - ctypes.byref(szFriendlyName), ctypes.sizeof(szFriendlyName) - 1, - None - ): - # Ignore ERROR_INSUFFICIENT_BUFFER - if ctypes.GetLastError() != ERROR_INSUFFICIENT_BUFFER: - #~ raise ctypes.WinError() - # not getting friendly name for com0com devices, try something else - szFriendlyName = ctypes.create_string_buffer(1024) - if SetupDiGetDeviceRegistryProperty( - g_hdi, - ctypes.byref(devinfo), - SPDRP_LOCATION_INFORMATION, - None, - ctypes.byref(szFriendlyName), ctypes.sizeof(szFriendlyName) - 1, - None - ): - port_name = "\\\\.\\" + szFriendlyName.value - order = None - else: - port_name = szFriendlyName.value - order = None - else: - try: - m = re.search(r"\((.*?(\d+))\)", szFriendlyName.value) - #~ print szFriendlyName.value, m.groups() - port_name = m.group(1) - order = int(m.group(2)) - except AttributeError, msg: - port_name = szFriendlyName.value - order = None - yield order, port_name, szFriendlyName.value, szHardwareID.value - - SetupDiDestroyDeviceInfoList(g_hdi) - - -if __name__ == '__main__': - import serial - print "-"*78 - print "Serial ports" - print "-"*78 - for order, port, desc, hwid in sorted(comports()): - print "%-10s: %s (%s) ->" % (port, desc, hwid), - try: - serial.Serial(port) # test open - except serial.serialutil.SerialException: - print "can't be openend" - else: - print "Ready" - print - # list of all ports the system knows - print "-"*78 - print "All serial ports (registry)" - print "-"*78 - for order, port, desc, hwid in sorted(comports(False)): - print "%-10s: %s (%s)" % (port, desc, hwid) diff --git a/mavlink/share/pyshared/pymavlink/tools/images/gtk-quit.gif b/mavlink/share/pyshared/pymavlink/tools/images/gtk-quit.gif deleted file mode 100644 index 3cce0867b..000000000 Binary files a/mavlink/share/pyshared/pymavlink/tools/images/gtk-quit.gif and /dev/null differ diff --git a/mavlink/share/pyshared/pymavlink/tools/images/media-playback-pause.gif b/mavlink/share/pyshared/pymavlink/tools/images/media-playback-pause.gif deleted file mode 100644 index 4dc439b78..000000000 Binary files a/mavlink/share/pyshared/pymavlink/tools/images/media-playback-pause.gif and /dev/null differ diff --git a/mavlink/share/pyshared/pymavlink/tools/images/media-playback-start.gif b/mavlink/share/pyshared/pymavlink/tools/images/media-playback-start.gif deleted file mode 100644 index ec4136399..000000000 Binary files a/mavlink/share/pyshared/pymavlink/tools/images/media-playback-start.gif and /dev/null differ diff --git a/mavlink/share/pyshared/pymavlink/tools/images/media-playback-stop.gif b/mavlink/share/pyshared/pymavlink/tools/images/media-playback-stop.gif deleted file mode 100644 index 306271a89..000000000 Binary files a/mavlink/share/pyshared/pymavlink/tools/images/media-playback-stop.gif and /dev/null differ diff --git a/mavlink/share/pyshared/pymavlink/tools/images/media-seek-backward.gif b/mavlink/share/pyshared/pymavlink/tools/images/media-seek-backward.gif deleted file mode 100644 index 23fcf6d82..000000000 Binary files a/mavlink/share/pyshared/pymavlink/tools/images/media-seek-backward.gif and /dev/null differ diff --git a/mavlink/share/pyshared/pymavlink/tools/images/media-seek-forward.gif b/mavlink/share/pyshared/pymavlink/tools/images/media-seek-forward.gif deleted file mode 100644 index 3439c54fd..000000000 Binary files a/mavlink/share/pyshared/pymavlink/tools/images/media-seek-forward.gif and /dev/null differ diff --git a/mavlink/share/pyshared/pymavlink/tools/images/player_end.gif b/mavlink/share/pyshared/pymavlink/tools/images/player_end.gif deleted file mode 100644 index 9496957d2..000000000 Binary files a/mavlink/share/pyshared/pymavlink/tools/images/player_end.gif and /dev/null differ diff --git a/mavlink/share/pyshared/pymavlink/tools/mavplayback.py b/mavlink/share/pyshared/pymavlink/tools/mavplayback.py deleted file mode 100644 index 033746697..000000000 --- a/mavlink/share/pyshared/pymavlink/tools/mavplayback.py +++ /dev/null @@ -1,246 +0,0 @@ -#!/usr/bin/env python - -''' -play back a mavlink log as a FlightGear FG NET stream, and as a -realtime mavlink stream - -Useful for visualising flights -''' - -import sys, time, os, struct -import Tkinter - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -import fgFDM - -from optparse import OptionParser -parser = OptionParser("mavplayback.py [options]") - -parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format") -parser.add_option("--condition",dest="condition", default=None, help="select packets by condition") -parser.add_option("--gpsalt", action='store_true', default=False, help="Use GPS altitude") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -parser.add_option("--out", help="MAVLink output port (IP:port)", - action='append', default=['127.0.0.1:14550']) -parser.add_option("--fgout", action='append', default=['127.0.0.1:5503'], - help="flightgear FDM NET output (IP:port)") -parser.add_option("--baudrate", type='int', default=57600, help='baud rate') -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 1: - parser.print_help() - sys.exit(1) - -filename = args[0] - - -def LoadImage(filename): - '''return an image from the images/ directory''' - app_dir = os.path.dirname(os.path.realpath(__file__)) - path = os.path.join(app_dir, 'files/images', filename) - return Tkinter.PhotoImage(file=path) - - -class App(): - def __init__(self, filename): - self.root = Tkinter.Tk() - - self.filesize = os.path.getsize(filename) - self.filepos = 0.0 - - self.mlog = mavutil.mavlink_connection(filename, planner_format=opts.planner, - robust_parsing=True) - self.mout = [] - for m in opts.out: - self.mout.append(mavutil.mavlink_connection(m, input=False, baud=opts.baudrate)) - - self.fgout = [] - for f in opts.fgout: - self.fgout.append(mavutil.mavudp(f, input=False)) - - self.fdm = fgFDM.fgFDM() - - self.msg = self.mlog.recv_match(condition=opts.condition) - if self.msg is None: - sys.exit(1) - self.last_timestamp = getattr(self.msg, '_timestamp') - - self.paused = False - - self.topframe = Tkinter.Frame(self.root) - self.topframe.pack(side=Tkinter.TOP) - - self.frame = Tkinter.Frame(self.root) - self.frame.pack(side=Tkinter.LEFT) - - self.slider = Tkinter.Scale(self.topframe, from_=0, to=1.0, resolution=0.01, - orient=Tkinter.HORIZONTAL, command=self.slew) - self.slider.pack(side=Tkinter.LEFT) - - self.clock = Tkinter.Label(self.topframe,text="") - self.clock.pack(side=Tkinter.RIGHT) - - self.playback = Tkinter.Spinbox(self.topframe, from_=0, to=20, increment=0.1, width=3) - self.playback.pack(side=Tkinter.BOTTOM) - self.playback.delete(0, "end") - self.playback.insert(0, 1) - - self.buttons = {} - self.button('quit', 'gtk-quit.gif', self.frame.quit) - self.button('pause', 'media-playback-pause.gif', self.pause) - self.button('rewind', 'media-seek-backward.gif', self.rewind) - self.button('forward', 'media-seek-forward.gif', self.forward) - self.button('status', 'Status', self.status) - self.flightmode = Tkinter.Label(self.frame,text="") - self.flightmode.pack(side=Tkinter.RIGHT) - - self.next_message() - self.root.mainloop() - - def button(self, name, filename, command): - '''add a button''' - try: - img = LoadImage(filename) - b = Tkinter.Button(self.frame, image=img, command=command) - b.image = img - except Exception: - b = Tkinter.Button(self.frame, text=filename, command=command) - b.pack(side=Tkinter.LEFT) - self.buttons[name] = b - - - def pause(self): - '''pause playback''' - self.paused = not self.paused - - def rewind(self): - '''rewind 10%''' - pos = int(self.mlog.f.tell() - 0.1*self.filesize) - if pos < 0: - pos = 0 - self.mlog.f.seek(pos) - self.find_message() - - def forward(self): - '''forward 10%''' - pos = int(self.mlog.f.tell() + 0.1*self.filesize) - if pos > self.filesize: - pos = self.filesize - 2048 - self.mlog.f.seek(pos) - self.find_message() - - def status(self): - '''show status''' - for m in sorted(self.mlog.messages.keys()): - print(str(self.mlog.messages[m])) - - - - def find_message(self): - '''find the next valid message''' - while True: - self.msg = self.mlog.recv_match(condition=opts.condition) - if self.msg is not None and self.msg.get_type() != 'BAD_DATA': - break - if self.mlog.f.tell() > self.filesize - 10: - self.paused = True - break - self.last_timestamp = getattr(self.msg, '_timestamp') - - def slew(self, value): - '''move to a given position in the file''' - if float(value) != self.filepos: - pos = float(value) * self.filesize - self.mlog.f.seek(int(pos)) - self.find_message() - - - def next_message(self): - '''called as each msg is ready''' - - msg = self.msg - if msg is None: - self.paused = True - - if self.paused: - self.root.after(100, self.next_message) - return - - speed = float(self.playback.get()) - timestamp = getattr(msg, '_timestamp') - - now = time.strftime("%H:%M:%S", time.localtime(timestamp)) - self.clock.configure(text=now) - - if speed == 0.0: - self.root.after(200, self.next_message) - else: - self.root.after(int(1000*(timestamp - self.last_timestamp) / speed), self.next_message) - self.last_timestamp = timestamp - - while True: - self.msg = self.mlog.recv_match(condition=opts.condition) - if self.msg is None and self.mlog.f.tell() > self.filesize - 10: - self.paused = True - return - if self.msg is not None and self.msg.get_type() != "BAD_DATA": - break - - pos = float(self.mlog.f.tell()) / self.filesize - self.slider.set(pos) - self.filepos = self.slider.get() - - if msg.get_type() != "BAD_DATA": - for m in self.mout: - m.write(msg.get_msgbuf().tostring()) - - if msg.get_type() == "GPS_RAW": - self.fdm.set('latitude', msg.lat, units='degrees') - self.fdm.set('longitude', msg.lon, units='degrees') - if opts.gpsalt: - self.fdm.set('altitude', msg.alt, units='meters') - - if msg.get_type() == "VFR_HUD": - if not opts.gpsalt: - self.fdm.set('altitude', msg.alt, units='meters') - self.fdm.set('num_engines', 1) - self.fdm.set('vcas', msg.airspeed, units='mps') - - if msg.get_type() == "ATTITUDE": - self.fdm.set('phi', msg.roll, units='radians') - self.fdm.set('theta', msg.pitch, units='radians') - self.fdm.set('psi', msg.yaw, units='radians') - self.fdm.set('phidot', msg.rollspeed, units='rps') - self.fdm.set('thetadot', msg.pitchspeed, units='rps') - self.fdm.set('psidot', msg.yawspeed, units='rps') - - if msg.get_type() == "RC_CHANNELS_SCALED": - self.fdm.set("right_aileron", msg.chan1_scaled*0.0001) - self.fdm.set("left_aileron", -msg.chan1_scaled*0.0001) - self.fdm.set("rudder", msg.chan4_scaled*0.0001) - self.fdm.set("elevator", msg.chan2_scaled*0.0001) - self.fdm.set('rpm', msg.chan3_scaled*0.01) - - if msg.get_type() == 'STATUSTEXT': - print("APM: %s" % msg.text) - - if msg.get_type() == 'SYS_STATUS': - self.flightmode.configure(text=self.mlog.flightmode) - - if msg.get_type() == "BAD_DATA": - if mavutil.all_printable(msg.data): - sys.stdout.write(msg.data) - sys.stdout.flush() - - if self.fdm.get('latitude') != 0: - for f in self.fgout: - f.write(self.fdm.pack()) - - -app=App(filename) -- cgit v1.2.3 From b74d4e2ba76d1fa3d6d099b63192b180b1af0a39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Jul 2014 16:59:44 +0200 Subject: Added NuttX submodule --- .gitignore | 1 - .gitmodules | 3 +++ NuttX | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) create mode 160000 NuttX diff --git a/.gitignore b/.gitignore index afd63e06a..8b09e4783 100644 --- a/.gitignore +++ b/.gitignore @@ -29,7 +29,6 @@ mavlink/include/mavlink/v0.9/ /nuttx-configs/px4io-v1/src/.depend /nuttx-configs/px4io-v1/src/Make.dep /nuttx-configs/px4io-v1/src/libboard.a -/NuttX /Documentation/doxy.log /Documentation/html/ /Documentation/doxygen*objdb*tmp diff --git a/.gitmodules b/.gitmodules index c5116c1cb..fe2d8713a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "mavlink/include/mavlink/v1.0"] path = mavlink/include/mavlink/v1.0 url = https://github.com/mavlink/c_library.git +[submodule "NuttX"] + path = NuttX + url = git://github.com/PX4/NuttX.git diff --git a/NuttX b/NuttX new file mode 160000 index 000000000..2a94cc8e5 --- /dev/null +++ b/NuttX @@ -0,0 +1 @@ +Subproject commit 2a94cc8e5babb7d5661aedc09201239d39644332 -- cgit v1.2.3 From 6c5c6ba9ee6cfff9ea748f812902922a5c7addba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Jul 2014 17:03:07 +0200 Subject: Add proper NuttX submodule checks --- Makefile | 10 ++++++---- Tools/check_submodules.sh | 8 ++++++++ 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index 0a8562251..7ea74eaae 100644 --- a/Makefile +++ b/Makefile @@ -104,7 +104,7 @@ DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) -all: $(DESIRED_FIRMWARES) +all: checksubmodules $(DESIRED_FIRMWARES) # # Copy FIRMWARES into the image directory. @@ -210,9 +210,11 @@ menuconfig: endif $(NUTTX_SRC): - @$(ECHO) "" - @$(ECHO) "NuttX sources missing - clone https://github.com/PX4/NuttX.git and try again." - @$(ECHO) "" + $(Q) if [ -d $(NUTTX_SRC) ]; then ./Tools/check_submodules.sh; else echo ""; echo ""; echo "NuttX submodule missing, doing auto checkout"; git submodule init; git submodule update; fi + +.PHONY: checksubmodules +checksubmodules: + $(Q) if [ -d $(MAVLINK_SRC) ]; then ./Tools/check_submodules.sh; else echo ""; echo ""; echo "MAVLink submodule missing, doing auto checkout"; git submodule init; git submodule update; fi # # Testing targets diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 52ea7a146..c431e8225 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -7,4 +7,12 @@ else exit 1 fi +STATUSRETVAL=$(git status --porcelain | grep -i "M NuttX") +if [ "$STATUSRETVAL" == "" ]; then + echo "checked NuttX submodule, correct version found" +else + echo "NuttX sub repo not at correct version. Try 'git submodule update'" + exit 1 +fi + exit 0 -- cgit v1.2.3 From 4c2cc65ca63141839ac4036cb8704b7ee7f27cb1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Jul 2014 17:18:54 +0200 Subject: Cleaning up sub modules --- .gitmodules | 5 ++++- Makefile | 10 ++++++---- Tools/check_submodules.sh | 8 ++++++++ makefiles/setup.mk | 1 + mavlink/include/mavlink/v1.0 | 1 + 5 files changed, 20 insertions(+), 5 deletions(-) create mode 160000 mavlink/include/mavlink/v1.0 diff --git a/.gitmodules b/.gitmodules index c5116c1cb..e352e15f5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ -[submodule "mavlink/include/mavlink/v1.0"] +[submodule "mavlink"] path = mavlink/include/mavlink/v1.0 url = https://github.com/mavlink/c_library.git +[submodule "mavlink/include/mavlink/v1.0"] + path = mavlink/include/mavlink/v1.0 + url = git://github.com/mavlink/c_library.git diff --git a/Makefile b/Makefile index 0a8562251..7ea74eaae 100644 --- a/Makefile +++ b/Makefile @@ -104,7 +104,7 @@ DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) -all: $(DESIRED_FIRMWARES) +all: checksubmodules $(DESIRED_FIRMWARES) # # Copy FIRMWARES into the image directory. @@ -210,9 +210,11 @@ menuconfig: endif $(NUTTX_SRC): - @$(ECHO) "" - @$(ECHO) "NuttX sources missing - clone https://github.com/PX4/NuttX.git and try again." - @$(ECHO) "" + $(Q) if [ -d $(NUTTX_SRC) ]; then ./Tools/check_submodules.sh; else echo ""; echo ""; echo "NuttX submodule missing, doing auto checkout"; git submodule init; git submodule update; fi + +.PHONY: checksubmodules +checksubmodules: + $(Q) if [ -d $(MAVLINK_SRC) ]; then ./Tools/check_submodules.sh; else echo ""; echo ""; echo "MAVLink submodule missing, doing auto checkout"; git submodule init; git submodule update; fi # # Testing targets diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 52ea7a146..c431e8225 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -7,4 +7,12 @@ else exit 1 fi +STATUSRETVAL=$(git status --porcelain | grep -i "M NuttX") +if [ "$STATUSRETVAL" == "" ]; then + echo "checked NuttX submodule, correct version found" +else + echo "NuttX sub repo not at correct version. Try 'git submodule update'" + exit 1 +fi + exit 0 diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 183b143d6..6a092ef6b 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -46,6 +46,7 @@ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ +export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/ export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/ export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/ diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 new file mode 160000 index 000000000..45a71d656 --- /dev/null +++ b/mavlink/include/mavlink/v1.0 @@ -0,0 +1 @@ +Subproject commit 45a71d6564bc5c47ed97d620089e17ca48bab73f -- cgit v1.2.3 From 9886bb8ca09dbf8a4b7fdd22e6d2dec9f9854eb9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Jul 2014 17:20:32 +0200 Subject: Fixed .gitmodules --- .gitmodules | 3 --- 1 file changed, 3 deletions(-) diff --git a/.gitmodules b/.gitmodules index e352e15f5..937db7cc6 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,3 @@ -[submodule "mavlink"] - path = mavlink/include/mavlink/v1.0 - url = https://github.com/mavlink/c_library.git [submodule "mavlink/include/mavlink/v1.0"] path = mavlink/include/mavlink/v1.0 url = git://github.com/mavlink/c_library.git -- cgit v1.2.3 From 72ccbe9c4ed304997fa5a4fdbf4bd668c69c35b0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Jul 2014 17:35:47 +0200 Subject: Fixed check tools --- Makefile | 4 ++-- Tools/check_submodules.sh | 36 ++++++++++++++++++++++++++---------- 2 files changed, 28 insertions(+), 12 deletions(-) diff --git a/Makefile b/Makefile index 7ea74eaae..7703cc04e 100644 --- a/Makefile +++ b/Makefile @@ -210,11 +210,11 @@ menuconfig: endif $(NUTTX_SRC): - $(Q) if [ -d $(NUTTX_SRC) ]; then ./Tools/check_submodules.sh; else echo ""; echo ""; echo "NuttX submodule missing, doing auto checkout"; git submodule init; git submodule update; fi + $(Q) (./Tools/check_submodules.sh) .PHONY: checksubmodules checksubmodules: - $(Q) if [ -d $(MAVLINK_SRC) ]; then ./Tools/check_submodules.sh; else echo ""; echo ""; echo "MAVLink submodule missing, doing auto checkout"; git submodule init; git submodule update; fi + $(Q) (./Tools/check_submodules.sh) # # Testing targets diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index c431e8225..d31c1588c 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -1,18 +1,34 @@ #!/bin/sh -STATUSRETVAL=$(git status --porcelain | grep -i "M mavlink/include/mavlink/v1.0") -if [ "$STATUSRETVAL" == "" ]; then - echo "checked mavlink submodule, correct version found" + +if [ -d NuttX/nuttx ]; + then + STATUSRETVAL=$(git status --porcelain | grep -i "mavlink/include/mavlink/v1.0") + if [ "$STATUSRETVAL" == "" ]; then + echo "Checked mavlink submodule, correct version found" + else + echo "mavlink sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi else - echo "mavlink sub repo not at correct version. Try 'git submodule update'" - exit 1 + git submodule init; + git submodule update; fi -STATUSRETVAL=$(git status --porcelain | grep -i "M NuttX") -if [ "$STATUSRETVAL" == "" ]; then - echo "checked NuttX submodule, correct version found" + +if [ -d mavlink/include/mavlink/v1.0 ]; + then + STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") + if [ "$STATUSRETVAL" == "" ]; then + echo "Checked NuttX submodule, correct version found" + else + echo "NuttX sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi else - echo "NuttX sub repo not at correct version. Try 'git submodule update'" - exit 1 + git submodule init; + git submodule update; fi exit 0 -- cgit v1.2.3 From cd9f4f33a593e8eea8a0314b776d1ad3ef847462 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Jul 2014 17:51:30 +0200 Subject: Turn instructions into a makefile command, allowing Windows GUI kids to create a make target in Eclipse just for this --- Makefile | 4 ++++ Tools/check_submodules.sh | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 7703cc04e..8ec5277e2 100644 --- a/Makefile +++ b/Makefile @@ -216,6 +216,10 @@ $(NUTTX_SRC): checksubmodules: $(Q) (./Tools/check_submodules.sh) +.PHONY: updatesubmodules +updatesubmodules: + $(Q) (git submodule update) + # # Testing targets # diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index d31c1588c..1156490d7 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -6,7 +6,7 @@ if [ -d NuttX/nuttx ]; if [ "$STATUSRETVAL" == "" ]; then echo "Checked mavlink submodule, correct version found" else - echo "mavlink sub repo not at correct version. Try 'git submodule update'" + echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi @@ -22,7 +22,7 @@ if [ -d mavlink/include/mavlink/v1.0 ]; if [ "$STATUSRETVAL" == "" ]; then echo "Checked NuttX submodule, correct version found" else - echo "NuttX sub repo not at correct version. Try 'git submodule update'" + echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi -- cgit v1.2.3 From 3b06a74074f71c0700c6e7e9b56954213142aaeb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Jul 2014 18:18:13 +0200 Subject: navigator: adapt offboard class to new NavigatorMode API --- src/modules/navigator/offboard.cpp | 27 ++++++++++++++++----------- src/modules/navigator/offboard.h | 16 +++------------- 2 files changed, 19 insertions(+), 24 deletions(-) diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index ef7d11a03..dcb5c6000 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -67,17 +67,27 @@ Offboard::~Offboard() { } -bool -Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Offboard::on_inactive() +{ +} + +void +Offboard::on_activation() +{ +} + +void +Offboard::on_active() { + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + bool updated; orb_check(_navigator->get_offboard_control_sp_sub(), &updated); if (updated) { update_offboard_control_setpoint(); } - bool changed = false; - /* copy offboard setpoints to the corresponding topics */ if (_navigator->get_control_mode()->flag_control_position_enabled && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { @@ -91,7 +101,7 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) pos_sp_triplet->current.valid = true; pos_sp_triplet->current.position_valid = true; - changed = true; + _navigator->set_position_setpoint_triplet_updated(); } else if (_navigator->get_control_mode()->flag_control_velocity_enabled && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) { @@ -105,16 +115,11 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) pos_sp_triplet->current.valid = true; pos_sp_triplet->current.velocity_valid = true; - changed = true; + _navigator->set_position_setpoint_triplet_updated(); } - return changed; } -void -Offboard::on_inactive() -{ -} void Offboard::update_offboard_control_setpoint() diff --git a/src/modules/navigator/offboard.h b/src/modules/navigator/offboard.h index 4d415dddb..66b923bdb 100644 --- a/src/modules/navigator/offboard.h +++ b/src/modules/navigator/offboard.h @@ -54,25 +54,15 @@ class Navigator; class Offboard : public NavigatorMode { public: - /** - * Constructor - */ Offboard(Navigator *navigator, const char *name); - /** - * Destructor - */ ~Offboard(); - /** - * This function is called while the mode is inactive - */ virtual void on_inactive(); - /** - * This function is called while the mode is active - */ - virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(); + + virtual void on_active(); private: void update_offboard_control_setpoint(); -- cgit v1.2.3 From f5c14ff140804b351464e53ec264945f9bcd48f3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 7 Jul 2014 10:17:39 -0700 Subject: Turn off noisier logging --- src/modules/mavlink/mavlink_ftp.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 2da381a2d..e0a52f393 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -210,7 +210,7 @@ MavlinkFTP::_workList(Request *req) return kErrNotDir; } - printf("FTP: list %s offset %d\n", dirPath, hdr->offset); + printf("FTP: list %s offset %d\n", dirPath, hdr->offset); ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -222,7 +222,7 @@ MavlinkFTP::_workList(Request *req) for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { - printf("FTP: list %s readdir_r failure\n", dirPath); + printf("FTP: list %s readdir_r failure\n", dirPath); errorCode = kErrIO; break; } @@ -258,7 +258,7 @@ MavlinkFTP::_workList(Request *req) // copy the name, which we know will fit strcpy((char *)&hdr->data[offset], entry.d_name); - printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); + //printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); offset += strlen(entry.d_name) + 1; } -- cgit v1.2.3 From 2eb018b2730d5b3ceeca430aabbf39cf560ce9fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Jul 2014 22:20:55 +0200 Subject: bugfixes for checks and tools --- Makefile | 1 + Tools/check_submodules.sh | 12 ++++++------ 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/Makefile b/Makefile index 8ec5277e2..8bf96ca23 100644 --- a/Makefile +++ b/Makefile @@ -218,6 +218,7 @@ checksubmodules: .PHONY: updatesubmodules updatesubmodules: + $(Q) (git submodule init) $(Q) (git submodule update) # diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 1156490d7..fb180ef47 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -2,11 +2,11 @@ if [ -d NuttX/nuttx ]; then - STATUSRETVAL=$(git status --porcelain | grep -i "mavlink/include/mavlink/v1.0") + STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") if [ "$STATUSRETVAL" == "" ]; then - echo "Checked mavlink submodule, correct version found" + echo "Checked NuttX submodule, correct version found" else - echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'" + echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi @@ -18,11 +18,11 @@ fi if [ -d mavlink/include/mavlink/v1.0 ]; then - STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") + STATUSRETVAL=$(git status --porcelain | grep -i "mavlink/include/mavlink/v1.0") if [ "$STATUSRETVAL" == "" ]; then - echo "Checked NuttX submodule, correct version found" + echo "Checked mavlink submodule, correct version found" else - echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'" + echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi -- cgit v1.2.3 From 680ebf29c3f8298e63d4c4d9c9076464e6f4b3c1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 7 Jul 2014 15:11:46 -0700 Subject: Fix compiler warnings --- makefiles/toolchain_gnu-arm-eabi.mk | 6 +++++- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 6 +----- src/modules/navigator/mission.cpp | 8 ++++---- src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/uORB/topics/telemetry_status.h | 10 ++++++++++ src/systemcmds/param/param.c | 1 - src/systemcmds/tests/test_mathlib.cpp | 2 ++ 7 files changed, 24 insertions(+), 13 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 808b635bb..d8d45d34e 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -139,7 +139,11 @@ ARCHWARNINGS = -Wall \ -Werror=format-security \ -Werror=array-bounds \ -Wfatal-errors \ - -Wformat=1 + -Wformat=1 \ + -Werror=unused-but-set-variable \ + -Werror=unused-variable \ + -Werror=double-promotion \ + -Werror=reorder # -Wcast-qual - generates spurious noreturn attribute warnings, try again later # -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code # -Wcast-align - would help catch bad casts in some cases, but generates too many false positives diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 973de1382..f33a1d780 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1742,7 +1742,6 @@ void AttPosEKF::FuseOptFlow() static float vd = 0.0f; static float pd = 0.0f; static float ptd = 0.0f; - static Vector3f delAng; static float R_LOS = 0.01f; static float losPred[2]; @@ -1820,9 +1819,6 @@ void AttPosEKF::FuseOptFlow() // calculate relative velocity in sensor frame relVelSensor = Tns*velNED_local; - // calculate delta angles in sensor axes - Vector3f delAngRel = Tbs*delAng; - // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes losPred[0] = relVelSensor.y/range; losPred[1] = -relVelSensor.x/range; @@ -1959,7 +1955,7 @@ void AttPosEKF::FuseOptFlow() } // normalise the quaternion states float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12) + if (quatMag > 1e-12f) { for (uint8_t j= 0; j<=3; j++) { diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cf6a764bf..d39ca6cf9 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -65,11 +65,11 @@ Mission::Mission(Navigator *navigator, const char *name) : _offboard_mission({0}), _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), + _need_takeoff(true), + _takeoff(false), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE), - _need_takeoff(true), - _takeoff(false) + _mission_type(MISSION_TYPE_NONE) { /* load initial params */ updateParams(); @@ -334,7 +334,7 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", takeoff_alt - _navigator->get_home_position()->alt); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index a1c6f09d0..4adf77dce 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -57,10 +57,10 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), + _mission_item({0}), _waypoint_position_reached(false), _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), - _mission_item({0}) + _time_first_inside_orbit(0) { } diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index c4b99d520..3347724a5 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -86,4 +86,14 @@ static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_I ORB_ID(telemetry_status_3), }; +// This is a hack to quiet an unused-variable warning for when telemetry_status.h is +// included but telemetry_status_orb_id is not referenced. The inline works if you +// choose to use it, but you can continue to just directly index into the array as well. +// If you don't use the inline this ends up being a no-op with no additional code emitted. +extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index); +extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index) +{ + return telemetry_status_orb_id[index]; +} + #endif /* TOPIC_TELEMETRY_STATUS_H */ diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 235ab7186..28e1b108b 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -231,7 +231,6 @@ do_show_print(void *arg, param_t param) /* start search */ const char *ss = search_string; const char *pp = p_name; - bool mismatch = false; /* XXX this comparison is only ok for trailing wildcards */ while (*ss != '\0' && *pp != '\0') { diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index df3ddb966..70d173fc9 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -98,6 +98,8 @@ int test_mathlib(int argc, char *argv[]) TEST_OP("Vector<3> length squared", v1.length_squared()); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" + // Need pragma here intead of moving variable out of TEST_OP and just reference because + // TEST_OP measures performance of vector operations. TEST_OP("Vector<3> element read", volatile float a = v1(0)); TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); #pragma GCC diagnostic pop -- cgit v1.2.3 From 6f1292838960ca9eb72dc1818250eb50f70ddb98 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 7 Jul 2014 20:00:44 -0700 Subject: Fix mainStateTransitionTest --- .../commander_tests/state_machine_helper_test.cpp | 213 ++++++++++++++------- 1 file changed, 147 insertions(+), 66 deletions(-) diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index ee0d08391..2e18c4284 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -183,12 +183,12 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) // Safety switch arming tests - { "transition: init to standby, no safety switch", + { "transition: standby to armed, no safety switch", { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, ARMING_STATE_ARMED, { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - { "transition: init to standby, safety switch off", + { "transition: standby to armed, safety switch off", { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, ARMING_STATE_ARMED, { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, @@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) armed.ready_to_arm = test->current_state.ready_to_arm; // Attempt transition - transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed); + transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */); // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); @@ -300,70 +300,151 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) bool StateMachineHelperTest::mainStateTransitionTest(void) { - struct vehicle_status_s current_state; - main_state_t new_main_state; + // This structure represent a single test case for testing Main State transitions. + typedef struct { + const char* assertMsg; // Text to show when test case fails + uint8_t condition_bits; // Bits for various condition_* values + main_state_t from_state; // State prior to transition request + main_state_t to_state; // State to transition to + transition_result_t expected_transition_result; // Expected result from main_state_transition call + } MainTransitionTest_t; + + // Bits for condition_bits + #define MTT_ALL_NOT_VALID 0 + #define MTT_ROTARY_WING 1 << 0 + #define MTT_LOC_ALT_VALID 1 << 1 + #define MTT_LOC_POS_VALID 1 << 2 + #define MTT_HOME_POS_VALID 1 << 3 + #define MTT_GLOBAL_POS_VALID 1 << 4 + + static const MainTransitionTest_t rgMainTransitionTests[] = { + + // TRANSITION_NOT_CHANGED tests + + { "no transition: identical states", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, + + // TRANSITION_CHANGED tests + + { "transition: MANUAL to ACRO", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED }, + + { "transition: ACRO to MANUAL", + MTT_ALL_NOT_VALID, + MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to AUTO_MISSION - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, + + { "transition: AUTO_MISSION to MANUAL - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to AUTO_LOITER - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, + + { "transition: AUTO_LOITER to MANUAL - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to AUTO_RTL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, + + { "transition: AUTO_RTL to MANUAL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, + MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to ALTCTL - not rotary", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + + { "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid", + MTT_ROTARY_WING | MTT_LOC_ALT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + + { "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid", + MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + + { "transition: ALTCTL to MANUAL - local altitude valid", + MTT_LOC_ALT_VALID, + MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to POSCTL - local position not valid, global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + + { "transition: MANUAL to POSCTL - local position valid, global position not valid", + MTT_LOC_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + + { "transition: POSCTL to MANUAL - local position valid, global position valid", + MTT_LOC_POS_VALID, + MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + // TRANSITION_DENIED tests + + { "no transition: MANUAL to AUTO_MISSION - global position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_LOITER - global position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid", + MTT_HOME_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid", + MTT_ROTARY_WING, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to POSCTL - local position not valid, global position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED }, + }; - // Identical states. - current_state.main_state = MAIN_STATE_MANUAL; - new_main_state = MAIN_STATE_MANUAL; - ut_assert("no transition: identical states", - TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // AUTO to MANUAL. - current_state.main_state = MAIN_STATE_AUTO; - new_main_state = MAIN_STATE_MANUAL; - ut_assert("transition changed: auto to manual", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // MANUAL to ALTCTRL. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTCTL; - ut_assert("tranisition: manual to altctrl", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state); - - // MANUAL to ALTCTRL, invalid local altitude. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTCTL; - ut_assert("no transition: invalid local altitude", - TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // MANUAL to POSCTRL. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSCTL; - ut_assert("transition: manual to posctrl", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state); - - // MANUAL to POSCTRL, invalid local position. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSCTL; - ut_assert("no transition: invalid position", - TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // MANUAL to AUTO. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_global_position_valid = true; - new_main_state = MAIN_STATE_AUTO; - ut_assert("transition: manual to auto", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); - - // MANUAL to AUTO, invalid global position. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_global_position_valid = false; - new_main_state = MAIN_STATE_AUTO; - ut_assert("no transition: invalid global position", - TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]); + for (size_t i=0; ifrom_state; + current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING; + current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID; + current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID; + current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID; + current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID; + + // Attempt transition + transition_result_t result = main_state_transition(¤t_state, test->to_state); + + // Validate result of transition + ut_assert(test->assertMsg, test->expected_transition_result == result); + if (test->expected_transition_result == result) { + if (test->expected_transition_result == TRANSITION_CHANGED) { + ut_assert(test->assertMsg, test->to_state == current_state.main_state); + } else { + ut_assert(test->assertMsg, test->from_state == current_state.main_state); + } + } + } + return true; } -- cgit v1.2.3 From c3826505ed36b4efa0272f863e45ea590b646e74 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 09:15:20 +0200 Subject: HIL: reduce excessive stack usage of driver --- src/drivers/hil/hil.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 55cc077fb..f17e99e9d 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -229,7 +229,7 @@ HIL::init() _task = task_spawn_cmd("fmuhil", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 1200, (main_t)&HIL::task_main_trampoline, nullptr); -- cgit v1.2.3 From 3b448078878b3b16b0145bc9c1288ce278c0acc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 09:15:47 +0200 Subject: Reduce excessive FMU stack usage --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0a4635728..d103935ae 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -329,7 +329,7 @@ PX4FMU::init() _task = task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 1600, (main_t)&PX4FMU::task_main_trampoline, nullptr); -- cgit v1.2.3 From 41f4e5a6bc3859d3e7682fafe01ad8a075121127 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 09:57:08 +0200 Subject: ardrone interface: Remove unused variable --- src/drivers/ardrone_interface/ardrone_motor_control.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 81f634992..fc017dd58 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -382,8 +382,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls float output_band = 0.0f; const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ - static bool initialized = false; - /* linearly scale the control inputs from 0 to startpoint_full_control */ if (motor_thrust < startpoint_full_control) { output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1 -- cgit v1.2.3 From 0d0c4c36265b044ba978cfe4dbe369e93aa25d44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 09:57:25 +0200 Subject: mc pos control: Fix reordering warnings --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 4e88e260c..9b0f69226 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -106,31 +106,31 @@ public: private: - bool _task_should_exit; /**< if true, task should exit */ + bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ int _mavlink_fd; /**< mavlink fd */ int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ - int _local_pos_sp_sub; /**< offboard local position setpoint */ - int _global_vel_sp_sub; /**< offboard global velocity setpoint */ + int _local_pos_sp_sub; /**< offboard local position setpoint */ + int _global_vel_sp_sub; /**< offboard global velocity setpoint */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_local_position_s _local_pos; /**< vehicle local position */ + struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ @@ -258,8 +258,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _manual_sub(-1), _arming_sub(-1), _local_pos_sub(-1), - _global_vel_sp_sub(-1), _pos_sp_triplet_sub(-1), + _global_vel_sp_sub(-1), /* publications */ _att_sp_pub(-1), -- cgit v1.2.3 From 9f1661f1f8808b7c83f73284cee9a1e9279165d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 09:58:08 +0200 Subject: Silence FTP transactions, be vocal about errors --- src/modules/mavlink/mavlink_ftp.cpp | 19 ++++++++++--------- src/modules/mavlink/mavlink_ftp.h | 27 ++++++++++++++------------- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 25 insertions(+), 23 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 57ae6076c..675a6870e 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -117,10 +117,10 @@ MavlinkFTP::_worker(Request *req) if (crc32(req->rawData(), req->dataSize()) != messageCRC) { errorCode = kErrNoRequest; goto out; - printf("ftp: bad crc\n"); + warnx("ftp: bad crc"); } - printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); + //printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); switch (hdr->opcode) { case kCmdNone: @@ -167,9 +167,9 @@ out: // handle success vs. error if (errorCode == kErrNone) { hdr->opcode = kRspAck; - printf("FTP: ack\n"); + //warnx("FTP: ack\n"); } else { - printf("FTP: nak %u\n", errorCode); + warnx("FTP: nak %u", errorCode); hdr->opcode = kRspNak; hdr->size = 1; hdr->data[0] = errorCode; @@ -206,11 +206,11 @@ MavlinkFTP::_workList(Request *req) DIR *dp = opendir(dirPath); if (dp == nullptr) { - printf("FTP: can't open path '%s'\n", dirPath); + warnx("FTP: can't open path '%s'", dirPath); return kErrNotDir; } - printf("FTP: list %s offset %d\n", dirPath, hdr->offset); + //warnx("FTP: list %s offset %d", dirPath, hdr->offset); ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -222,7 +222,7 @@ MavlinkFTP::_workList(Request *req) for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { - printf("FTP: list %s readdir_r failure\n", dirPath); + warnx("FTP: list %s readdir_r failure\n", dirPath); errorCode = kErrIO; break; } @@ -304,19 +304,20 @@ MavlinkFTP::_workRead(Request *req) } // Seek to the specified position - printf("Seek %d\n", hdr->offset); + //warnx("seek %d", hdr->offset); if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { // Unable to see to the specified location + warnx("seek fail"); return kErrEOF; } int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); if (bytes_read < 0) { // Negative return indicates error other than eof + warnx("read fail %d", bytes_read); return kErrIO; } - printf("Read success %d\n", bytes_read); hdr->size = bytes_read; return kErrNone; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index e22e61553..59237a4c4 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -147,20 +147,21 @@ private: unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), _mavlink->get_channel(), &msg, sequence(), rawData()); - _mavlink->lockMessageBufferMutex(); - bool fError = _mavlink->message_buffer_write(&msg, len); - _mavlink->unlockMessageBufferMutex(); - - if (!fError) { + _mavlink->lockMessageBufferMutex(); + bool success = _mavlink->message_buffer_write(&msg, len); + _mavlink->unlockMessageBufferMutex(); + + if (!success) { warnx("FTP TX ERR"); - } else { - warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", - _mavlink->get_system_id(), - _mavlink->get_component_id(), - _mavlink->get_channel(), - len, - msg.checksum); - } + } + // else { + // warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", + // _mavlink->get_system_id(), + // _mavlink->get_component_id(), + // _mavlink->get_channel(), + // len, + // msg.checksum); + // } } uint8_t *rawData() { return &_message.data[0]; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 81966caf8..484886682 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -442,7 +442,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; - warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", manual.x, manual.y, manual.r, manual.z); + warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); -- cgit v1.2.3 From 86932423af88f9ed0a53e0fab6b12160ff0b3efb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 10:50:59 +0200 Subject: update mavlink to latest master --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 45a71d656..3711190d2 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 45a71d6564bc5c47ed97d620089e17ca48bab73f +Subproject commit 3711190d23d9928ea0687e00621c8d9ecf145f50 -- cgit v1.2.3 From 37b4cdfce21ff6a7374599a6706ad387bd359515 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 25 Mar 2014 10:56:33 +1100 Subject: board_serial: use a uint8_t buffer we should not be using 'char' for binary APIs, as the C standard does not specify if it is signed or unsigned, so results may not be consistent --- src/drivers/px4fmu/fmu.cpp | 2 +- src/modules/systemlib/board_serial.c | 8 ++++---- src/modules/systemlib/board_serial.h | 2 +- src/modules/systemlib/otp.h | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0a4635728..39cc32967 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1784,7 +1784,7 @@ fmu_main(int argc, char *argv[]) } if (!strcmp(verb, "id")) { - char id[12]; + uint8_t id[12]; (void)get_board_serial(id); errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X", diff --git a/src/modules/systemlib/board_serial.c b/src/modules/systemlib/board_serial.c index ad8c2bf83..182fd15c6 100644 --- a/src/modules/systemlib/board_serial.c +++ b/src/modules/systemlib/board_serial.c @@ -44,11 +44,11 @@ #include "board_config.h" #include "board_serial.h" -int get_board_serial(char *serialid) +int get_board_serial(uint8_t *serialid) { - const volatile unsigned *udid_ptr = (const unsigned *)UDID_START; + const volatile uint32_t *udid_ptr = (const uint32_t *)UDID_START; union udid id; - val_read((unsigned *)&id, udid_ptr, sizeof(id)); + val_read((uint32_t *)&id, udid_ptr, sizeof(id)); /* Copy the serial from the chips non-write memory and swap endianess */ @@ -57,4 +57,4 @@ int get_board_serial(char *serialid) serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8]; return 0; -} \ No newline at end of file +} diff --git a/src/modules/systemlib/board_serial.h b/src/modules/systemlib/board_serial.h index b14bb4376..873d9657b 100644 --- a/src/modules/systemlib/board_serial.h +++ b/src/modules/systemlib/board_serial.h @@ -44,6 +44,6 @@ __BEGIN_DECLS -__EXPORT int get_board_serial(char *serialid); +__EXPORT int get_board_serial(uint8_t *serialid); __END_DECLS diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h index f10e129d8..273b064f0 100644 --- a/src/modules/systemlib/otp.h +++ b/src/modules/systemlib/otp.h @@ -125,7 +125,7 @@ struct otp_lock { #pragma pack(push, 1) union udid { uint32_t serial[3]; - char data[12]; + uint8_t data[12]; }; #pragma pack(pop) -- cgit v1.2.3 From ef79d032760e83850a9dbcbe5ae34c8b72f5fb4f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 14 May 2014 16:01:10 +1000 Subject: mpu6000: allow disabling of on-sensor low pass filter used for vibration testing --- src/drivers/mpu6000/mpu6000.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 0edec3d0e..fb4acc360 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -666,7 +666,9 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) /* choose next highest filter frequency available */ - if (frequency_hz <= 5) { + if (frequency_hz == 0) { + filter = BITS_DLPF_CFG_2100HZ_NOLPF; + } else if (frequency_hz <= 5) { filter = BITS_DLPF_CFG_5HZ; } else if (frequency_hz <= 10) { filter = BITS_DLPF_CFG_10HZ; @@ -922,10 +924,11 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSLOWPASS: - - // XXX decide on relationship of both filters - // i.e. disable the on-chip filter - //_set_dlpf_filter((uint16_t)arg); + if (arg == 0) { + // allow disabling of on-chip filter using + // zero as desired filter frequency + _set_dlpf_filter(0); + } _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); @@ -1009,8 +1012,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); - // XXX check relation to the internal lowpass - //_set_dlpf_filter((uint16_t)arg); + if (arg == 0) { + // allow disabling of on-chip filter using 0 + // as desired frequency + _set_dlpf_filter(0); + } return OK; case GYROIOCSSCALE: -- cgit v1.2.3 From 07855120268cef79e9d23d7cd091c526fa4622af Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 19 May 2014 16:49:41 +1000 Subject: px_uploader: added ModemManager warning --- Tools/px_uploader.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 985e6ffd9..a113f5628 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -63,6 +63,7 @@ import zlib import base64 import time import array +import os from sys import platform as _platform @@ -449,6 +450,12 @@ parser.add_argument('--baud', action="store", type=int, default=115200, help="Ba parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") args = parser.parse_args() +# warn people about ModemManager which interferes badly with Pixhawk +if os.path.exists("/usr/sbin/ModemManager"): + print("=======================================================================") + print("WARNING: You should uninstall ModemManager as it conflicts with Pixhawk") + print("=======================================================================") + # Load the firmware file fw = firmware(args.firmware) print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) -- cgit v1.2.3 From d6999384ceb905f82896b99dda6341c7b615ba7c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 11:42:46 +0200 Subject: Improve modem manager warning to avoid having smart people tell us we have Linux compatibility issues while we actually do not. --- Tools/px_uploader.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index a113f5628..cd7884f6d 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -452,9 +452,9 @@ args = parser.parse_args() # warn people about ModemManager which interferes badly with Pixhawk if os.path.exists("/usr/sbin/ModemManager"): - print("=======================================================================") - print("WARNING: You should uninstall ModemManager as it conflicts with Pixhawk") - print("=======================================================================") + print("==========================================================================================================") + print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)") + print("==========================================================================================================") # Load the firmware file fw = firmware(args.firmware) -- cgit v1.2.3 From db304480d996c134fbc2525e5bc96a08178275a9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 May 2014 11:14:38 +1000 Subject: lsm303d: disable check_extremes code this could trigger with a bungee launch, and could cause higher latency due to SD card writes --- src/drivers/lsm303d/lsm303d.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8bf76dcc3..56baf3274 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -75,6 +75,10 @@ #endif static const int ERROR = -1; +// enable this to debug the buggy lsm303d sensor in very early +// prototype pixhawk boards +#define CHECK_EXTREMES 0 + /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -830,7 +834,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { +#if CHECK_EXTREMES check_extremes(arb); +#endif ret += sizeof(*arb); arb++; } -- cgit v1.2.3 From 644d4bb3dc6186d7908ed0aa75d973cfc0826253 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 10:06:52 +1000 Subject: FMUv2: added defines for FMUv3 sensors this enables EXT0 to EXT3 on external SPI bus, and gives correct names for FMUv3 board --- src/drivers/boards/px4fmu-v2/board_config.h | 10 ++++++++++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 24 ++++++++++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 36eb7bec4..2ed51d7b1 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -108,6 +108,8 @@ __BEGIN_DECLS #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) #define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) #define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) +#define GPIO_SPI_CS_EXT2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define PX4_SPI_BUS_SENSORS 1 #define PX4_SPI_BUS_EXT 4 @@ -121,6 +123,14 @@ __BEGIN_DECLS /* External bus */ #define PX4_SPIDEV_EXT0 1 #define PX4_SPIDEV_EXT1 2 +#define PX4_SPIDEV_EXT2 3 +#define PX4_SPIDEV_EXT3 4 + +/* FMUv3 SPI on external bus */ +#define PX4_SPIDEV_EXT_MPU PX4_SPIDEV_EXT0 +#define PX4_SPIDEV_EXT_BARO PX4_SPIDEV_EXT1 +#define PX4_SPIDEV_EXT_ACCEL_MAG PX4_SPIDEV_EXT2 +#define PX4_SPIDEV_EXT_GYRO PX4_SPIDEV_EXT3 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 01dbd6e77..8c37d31a7 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -98,8 +98,12 @@ __EXPORT void weak_function stm32_spiinitialize(void) #ifdef CONFIG_STM32_SPI4 stm32_configgpio(GPIO_SPI_CS_EXT0); stm32_configgpio(GPIO_SPI_CS_EXT1); + stm32_configgpio(GPIO_SPI_CS_EXT2); + stm32_configgpio(GPIO_SPI_CS_EXT3); stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); #endif } @@ -174,12 +178,32 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, /* Making sure the other peripherals are not selected */ stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected); stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); break; case PX4_SPIDEV_EXT1: /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + break; + + case PX4_SPIDEV_EXT2: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + break; + + case PX4_SPIDEV_EXT3: + /* Making sure the other peripherals are not selected */ stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, !selected); break; default: -- cgit v1.2.3 From fea4845ed97ca5219ceb8af0b0fb6d68603eea17 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:13:05 +1000 Subject: SPI: make _bus protected this allows runtime use of internal/external bus to determine if DRDY should be used on the L3GD20 --- src/drivers/device/spi.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 299575dc5..03986da1e 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -124,11 +124,13 @@ protected: LockMode locking_mode; /**< selected locking mode */ private: - int _bus; enum spi_dev_e _device; enum spi_mode_e _mode; uint32_t _frequency; struct spi_dev_s *_dev; + +protected: + int _bus; }; } // namespace device -- cgit v1.2.3 From e7360f40169a0d467448bca8f85d32a84025ca4e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:13:58 +1000 Subject: l3gd20: added -X switch for external bus --- src/drivers/l3gd20/l3gd20.cpp | 50 ++++++++++++++++++++++++++++++++++++------- 1 file changed, 42 insertions(+), 8 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 37e72388b..06b36d688 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -821,7 +822,7 @@ L3GD20::measure() // if the gyro doesn't have any data ready then re-schedule // for 100 microseconds later. This ensures we don't double // read a value and then miss the next value - if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { + if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { perf_count(_reschedules); hrt_call_delay(&_call, 100); return; @@ -983,7 +984,7 @@ void info(); * Start the driver. */ void -start() +start(bool external_bus) { int fd; @@ -991,7 +992,15 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + if (external_bus) { +#ifdef PX4_SPI_BUS_EXT + g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO); +#else + errx(0, "External SPI not available"); +#endif + } else { + g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + } if (g_dev == nullptr) goto fail; @@ -1106,32 +1115,57 @@ info() } // namespace +void +l3gd20_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset'"); + warnx("options:"); + warnx(" -X (external bus)"); +} + int l3gd20_main(int argc, char *argv[]) { + bool external_bus = false; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "X")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + default: + l3gd20_usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - l3gd20::start(); + if (!strcmp(verb, "start")) + l3gd20::start(external_bus); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) l3gd20::test(); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) l3gd20::reset(); /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(verb, "info")) l3gd20::info(); errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); -- cgit v1.2.3 From e0dbc82d84c2ad0e36b88f6b6d3cbfab866b4c44 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:14:17 +1000 Subject: lsm303d: added -X option for external bus --- src/drivers/lsm303d/lsm303d.cpp | 51 ++++++++++++++++++++++++++++++++--------- 1 file changed, 40 insertions(+), 11 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 56baf3274..0428901a3 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -85,6 +86,7 @@ static const int ERROR = -1; #define ADDR_INCREMENT (1<<6) #define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" +#define LSM303D_DEVICE_PATH_ACCEL_EXT "/dev/lsm303d_accel_ext" #define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" /* register addresses: A: accel, M: mag, T: temp */ @@ -1791,16 +1793,18 @@ void logging(); * Start the driver. */ void -start() +start(bool external_bus) { int fd, fd_mag; - if (g_dev != nullptr) errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); - + if (external_bus) { + g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG); + } else { + g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + } if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); goto fail; @@ -1998,44 +2002,69 @@ logging() } // namespace +void +lsm303d_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'"); + warnx("options:"); + warnx(" -X (external bus)"); +} + int lsm303d_main(int argc, char *argv[]) { + bool external_bus = false; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "X")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + default: + lsm303d_usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - lsm303d::start(); + if (!strcmp(verb, "start")) + lsm303d::start(external_bus); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) lsm303d::test(); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) lsm303d::reset(); /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(verb, "info")) lsm303d::info(); /* * dump device registers */ - if (!strcmp(argv[1], "regdump")) + if (!strcmp(verb, "regdump")) lsm303d::regdump(); /* * dump device registers */ - if (!strcmp(argv[1], "logging")) + if (!strcmp(verb, "logging")) lsm303d::logging(); errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); -- cgit v1.2.3 From 541dc1825cfca3724a7fbe08abfdf88b881b0d3a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:14:33 +1000 Subject: mpu6000: added -X option for external bus --- src/drivers/mpu6000/mpu6000.cpp | 48 +++++++++++++++++++++++++++++++++++------ 1 file changed, 41 insertions(+), 7 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index fb4acc360..a1c169ffc 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -1424,7 +1425,7 @@ void info(); * Start the driver. */ void -start() +start(bool external_bus) { int fd; @@ -1433,7 +1434,15 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU); + if (external_bus) { +#ifdef PX4_SPI_BUS_EXT + g_dev = new MPU6000(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_MPU); +#else + errx(0, "External SPI not available"); +#endif + } else { + g_dev = new MPU6000(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_MPU); + } if (g_dev == nullptr) goto fail; @@ -1578,32 +1587,57 @@ info() } // namespace +void +mpu6000_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset'"); + warnx("options:"); + warnx(" -X (external bus)"); +} + int mpu6000_main(int argc, char *argv[]) { + bool external_bus = false; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "X")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + default: + mpu6000_usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - mpu6000::start(); + if (!strcmp(verb, "start")) + mpu6000::start(external_bus); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) mpu6000::test(); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) mpu6000::reset(); /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(verb, "info")) mpu6000::info(); errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); -- cgit v1.2.3 From ab90fe783287a068ee3654e488ea9144077586ab Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:15:43 +1000 Subject: ms5611: added -X option for external SPI bus --- src/drivers/ms5611/ms5611.cpp | 46 ++++++++++++++++++++++++++++++--------- src/drivers/ms5611/ms5611.h | 2 +- src/drivers/ms5611/ms5611_spi.cpp | 10 ++++++--- 3 files changed, 44 insertions(+), 14 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 1ce93aeea..7a8d2eecf 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include @@ -775,7 +776,7 @@ namespace ms5611 MS5611 *g_dev; -void start(); +void start(bool external_bus); void test(); void reset(); void info(); @@ -832,7 +833,7 @@ crc4(uint16_t *n_prom) * Start the driver. */ void -start() +start(bool external_bus) { int fd; prom_u prom_buf; @@ -845,7 +846,7 @@ start() /* create the driver, try SPI first, fall back to I2C if unsuccessful */ if (MS5611_spi_interface != nullptr) - interface = MS5611_spi_interface(prom_buf); + interface = MS5611_spi_interface(prom_buf, external_bus); if (interface == nullptr && (MS5611_i2c_interface != nullptr)) interface = MS5611_i2c_interface(prom_buf); @@ -1058,41 +1059,66 @@ calibrate(unsigned altitude) } // namespace +void +ms5611_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); + warnx("options:"); + warnx(" -X (external bus)"); +} + int ms5611_main(int argc, char *argv[]) { + bool external_bus = false; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "X")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + default: + ms5611_usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - ms5611::start(); + if (!strcmp(verb, "start")) + ms5611::start(external_bus); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) ms5611::test(); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) ms5611::reset(); /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(verb, "info")) ms5611::info(); /* * Perform MSL pressure calibration given an altitude in metres */ - if (!strcmp(argv[1], "calibrate")) { + if (!strcmp(verb, "calibrate")) { if (argc < 2) errx(1, "missing altitude"); - long altitude = strtol(argv[2], nullptr, 10); + long altitude = strtol(argv[optind+1], nullptr, 10); ms5611::calibrate(altitude); } diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index 76fb84de8..f0b3fd61d 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom); } /* namespace */ /* interface factories */ -extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf) weak_function; +extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) weak_function; extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function; diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 8759d16a1..00d016aed 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -62,7 +62,7 @@ #ifdef PX4_SPIDEV_BARO -device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf); +device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus); class MS5611_SPI : public device::SPI { @@ -115,9 +115,13 @@ private: }; device::Device * -MS5611_spi_interface(ms5611::prom_u &prom_buf) +MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) { - return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); + if (external_bus) { + return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); + } else { + return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); + } } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : -- cgit v1.2.3 From 19dbbf17e8b2fd37e43aa1a5cb6ae8c39012ab0f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 13:51:04 +1000 Subject: mpu6000: allow for two mpu6000 instances, one internal, one external split g_dev into g_dev_int and g_dev_ext --- src/drivers/mpu6000/mpu6000.cpp | 82 +++++++++++++++++++++++------------------ 1 file changed, 46 insertions(+), 36 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index a1c169ffc..7f9e9fde4 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -78,6 +78,8 @@ #define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel" #define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro" +#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu6000_accel_ext" +#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu6000_gyro_ext" // MPU 6000 registers #define MPUREG_WHOAMI 0x75 @@ -178,7 +180,7 @@ class MPU6000_gyro; class MPU6000 : public device::SPI { public: - MPU6000(int bus, spi_dev_e device); + MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device); virtual ~MPU6000(); virtual int init(); @@ -346,7 +348,7 @@ private: class MPU6000_gyro : public device::CDev { public: - MPU6000_gyro(MPU6000 *parent); + MPU6000_gyro(MPU6000 *parent, const char *path); ~MPU6000_gyro(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); @@ -369,9 +371,9 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } -MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), - _gyro(new MPU6000_gyro(this)), +MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device) : + SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), _call_interval(0), _accel_reports(nullptr), @@ -1357,8 +1359,8 @@ MPU6000::print_info() _gyro_reports->print_info("gyro queue"); } -MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : - CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), +MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : + CDev("MPU6000_gyro", path), _parent(parent), _gyro_topic(-1), _gyro_class_instance(-1) @@ -1414,12 +1416,13 @@ MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) namespace mpu6000 { -MPU6000 *g_dev; +MPU6000 *g_dev_int; // on internal bus +MPU6000 *g_dev_ext; // on external bus -void start(); -void test(); -void reset(); -void info(); +void start(bool); +void test(bool); +void reset(bool); +void info(bool); /** * Start the driver. @@ -1428,30 +1431,33 @@ void start(bool external_bus) { int fd; + MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; + const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; - if (g_dev != nullptr) + if (*g_dev_ptr != nullptr) /* if already started, the still command succeeded */ errx(0, "already started"); /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - g_dev = new MPU6000(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_MPU); + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU); #else errx(0, "External SPI not available"); #endif } else { - g_dev = new MPU6000(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_MPU); + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU); } - if (g_dev == nullptr) + if (*g_dev_ptr == nullptr) goto fail; - if (OK != g_dev->init()) + if (OK != (*g_dev_ptr)->init()) goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); + fd = open(path_accel, O_RDONLY); if (fd < 0) goto fail; @@ -1464,9 +1470,9 @@ start(bool external_bus) exit(0); fail: - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + if (*g_dev_ptr != nullptr) { + delete (*g_dev_ptr); + *g_dev_ptr = nullptr; } errx(1, "driver start failed"); @@ -1478,24 +1484,26 @@ fail: * and automatic modes. */ void -test() +test(bool external_bus) { + const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; accel_report a_report; gyro_report g_report; ssize_t sz; /* get the driver */ - int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); + int fd = open(path_accel, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", - MPU_DEVICE_PATH_ACCEL); + path_accel); /* get the driver */ - int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY); + int fd_gyro = open(path_gyro, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", MPU_DEVICE_PATH_GYRO); + err(1, "%s open failed", path_gyro); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -1543,7 +1551,7 @@ test() /* XXX add poll-rate tests here too */ - reset(); + reset(external_bus); errx(0, "PASS"); } @@ -1551,9 +1559,10 @@ test() * Reset the driver. */ void -reset() +reset(bool external_bus) { - int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); + const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + int fd = open(path_accel, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1573,13 +1582,14 @@ reset() * Print a little info about the driver. */ void -info() +info(bool external_bus) { - if (g_dev == nullptr) + MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; + if (*g_dev_ptr == nullptr) errx(1, "driver not running"); - printf("state @ %p\n", g_dev); - g_dev->print_info(); + printf("state @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_info(); exit(0); } @@ -1626,19 +1636,19 @@ mpu6000_main(int argc, char *argv[]) * Test the driver/device. */ if (!strcmp(verb, "test")) - mpu6000::test(); + mpu6000::test(external_bus); /* * Reset the driver. */ if (!strcmp(verb, "reset")) - mpu6000::reset(); + mpu6000::reset(external_bus); /* * Print driver information. */ if (!strcmp(verb, "info")) - mpu6000::info(); + mpu6000::info(external_bus); errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } -- cgit v1.2.3 From e4e152a85b5a29cada6559197580a9dce93e45e3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 11:30:05 +1000 Subject: FMUv2: added define for PX4_I2C_BUS_ONBOARD needed for hmc5883 on main bus (for FMUv3) --- src/drivers/boards/px4fmu-v2/board_config.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 2ed51d7b1..0190a5b5b 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -134,7 +134,8 @@ __BEGIN_DECLS /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 -#define PX4_I2C_BUS_LED 2 +#define PX4_I2C_BUS_ONBOARD 2 +#define PX4_I2C_BUS_LED PX4_I2C_BUS_ONBOARD /* Devices on the onboard bus. * -- cgit v1.2.3 From f56724f7dff1f6d1167f4fd61015ffe24a64c8c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 14:02:05 +1000 Subject: lib/conversion: added rotate_3f() will be used for user specified rotations in sensor drivers --- src/lib/conversion/rotation.cpp | 142 ++++++++++++++++++++++++++++++++++++++++ src/lib/conversion/rotation.h | 8 +++ 2 files changed, 150 insertions(+) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 614877b18..e17715733 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -49,3 +49,145 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix) rot_matrix->from_euler(roll, pitch, yaw); } + +#define HALF_SQRT_2 0.70710678118654757f + +__EXPORT void +rotate_3f(enum Rotation rot, float &x, float &y, float &z) +{ + float tmp; + switch (rot) { + case ROTATION_NONE: + case ROTATION_MAX: + return; + case ROTATION_YAW_45: { + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_YAW_90: { + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_YAW_135: { + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_YAW_180: + x = -x; y = -y; + return; + case ROTATION_YAW_225: { + tmp = HALF_SQRT_2*(y - x); + y = -HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_YAW_270: { + tmp = x; x = y; y = -tmp; + return; + } + case ROTATION_YAW_315: { + tmp = HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(y - x); + x = tmp; + return; + } + case ROTATION_ROLL_180: { + y = -y; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_45: { + tmp = HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_90: { + tmp = x; x = y; y = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_135: { + tmp = HALF_SQRT_2*(y - x); + y = HALF_SQRT_2*(y + x); + x = tmp; z = -z; + return; + } + case ROTATION_PITCH_180: { + x = -x; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_225: { + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(y - x); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_270: { + tmp = x; x = -y; y = -tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_315: { + tmp = HALF_SQRT_2*(x - y); + y = -HALF_SQRT_2*(x + y); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_90: { + tmp = z; z = y; y = -tmp; + return; + } + case ROTATION_ROLL_90_YAW_45: { + tmp = z; z = y; y = -tmp; + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_ROLL_90_YAW_90: { + tmp = z; z = y; y = -tmp; + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_ROLL_90_YAW_135: { + tmp = z; z = y; y = -tmp; + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_ROLL_270: { + tmp = z; z = -y; y = tmp; + return; + } + case ROTATION_ROLL_270_YAW_45: { + tmp = z; z = -y; y = tmp; + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_ROLL_270_YAW_90: { + tmp = z; z = -y; y = tmp; + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_ROLL_270_YAW_135: { + tmp = z; z = -y; y = tmp; + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_PITCH_90: { + tmp = z; z = -x; x = tmp; + return; + } + case ROTATION_PITCH_270: { + tmp = z; z = x; x = -tmp; + return; + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 0c56494c5..5187b448f 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -118,4 +118,12 @@ const rot_lookup_t rot_lookup[] = { __EXPORT void get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); + +/** + * rotate a 3 element float vector in-place + */ +__EXPORT void +rotate_3f(enum Rotation rot, float &x, float &y, float &z); + + #endif /* ROTATION_H_ */ -- cgit v1.2.3 From c681d6621d8e4b29c3be8d8b94bf765b42f10e49 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 14:02:28 +1000 Subject: mpu6000: added -R rotation option --- src/drivers/mpu6000/mpu6000.cpp | 32 +++++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 7f9e9fde4..f6e00feee 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -180,7 +181,7 @@ class MPU6000_gyro; class MPU6000 : public device::SPI { public: - MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device); + MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation); virtual ~MPU6000(); virtual int init(); @@ -235,6 +236,8 @@ private: math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_z; + enum Rotation _rotation; + /** * Start automatic measurement. */ @@ -371,7 +374,7 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } -MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device) : +MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) : SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), @@ -394,7 +397,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ) + _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _rotation(rotation) { // disable debug() calls _debug_enabled = false; @@ -1304,6 +1308,9 @@ MPU6000::measure() arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); + // apply user specified rotation + rotate_3f(_rotation, arb.x, arb.y, arb.z); + arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1322,6 +1329,9 @@ MPU6000::measure() grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); + // apply user specified rotation + rotate_3f(_rotation, grb.x, grb.y, grb.z); + grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; @@ -1419,7 +1429,7 @@ namespace mpu6000 MPU6000 *g_dev_int; // on internal bus MPU6000 *g_dev_ext; // on external bus -void start(bool); +void start(bool, enum Rotation); void test(bool); void reset(bool); void info(bool); @@ -1428,7 +1438,7 @@ void info(bool); * Start the driver. */ void -start(bool external_bus) +start(bool external_bus, enum Rotation rotation) { int fd; MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; @@ -1442,12 +1452,12 @@ start(bool external_bus) /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU); + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); #else errx(0, "External SPI not available"); #endif } else { - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU); + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); } if (*g_dev_ptr == nullptr) @@ -1610,13 +1620,17 @@ mpu6000_main(int argc, char *argv[]) { bool external_bus = false; int ch; + enum Rotation rotation = ROTATION_NONE; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "X")) != EOF) { + while ((ch = getopt(argc, argv, "XR:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; default: mpu6000_usage(); exit(0); @@ -1630,7 +1644,7 @@ mpu6000_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) - mpu6000::start(external_bus); + mpu6000::start(external_bus, rotation); /* * Test the driver/device. -- cgit v1.2.3 From 1dfc7bad7b04af74d154b9c4bd3886f346f38d72 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 14:06:52 +1000 Subject: Merged lsm303d update, keeping default frequency --- src/drivers/lsm303d/lsm303d.cpp | 33 +++++++++++++++++++++++---------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 0428901a3..f25ac8f87 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -69,6 +69,7 @@ #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -222,7 +223,7 @@ class LSM303D_mag; class LSM303D : public device::SPI { public: - LSM303D(int bus, const char* path, spi_dev_e device); + LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation); virtual ~LSM303D(); virtual int init(); @@ -311,7 +312,8 @@ private: uint64_t _last_log_us; uint64_t _last_log_sync_us; uint64_t _last_log_reg_us; - uint64_t _last_log_alarm_us; + uint64_t _last_log_alarm_us; + enum Rotation _rotation; /** * Start automatic measurement. @@ -491,7 +493,7 @@ private: }; -LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : +LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), _mag(new LSM303D_mag(this)), _call_accel_interval(0), @@ -525,7 +527,8 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _last_log_us(0), _last_log_sync_us(0), _last_log_reg_us(0), - _last_log_alarm_us(0) + _last_log_alarm_us(0), + _rotation(rotation) { // enable debug() calls _debug_enabled = true; @@ -1541,6 +1544,9 @@ LSM303D::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); + // apply user specified rotation + rotate_3f(_rotation, accel_report.x, accel_report.y, accel_report.z); + accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; @@ -1617,6 +1623,9 @@ LSM303D::mag_measure() mag_report.scaling = _mag_range_scale; mag_report.range_ga = (float)_mag_range_ga; + // apply user specified rotation + rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z); + _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ @@ -1782,7 +1791,7 @@ namespace lsm303d LSM303D *g_dev; -void start(); +void start(bool external_bus, enum Rotation rotation); void test(); void reset(); void info(); @@ -1793,7 +1802,7 @@ void logging(); * Start the driver. */ void -start(bool external_bus) +start(bool external_bus, enum Rotation rotation) { int fd, fd_mag; if (g_dev != nullptr) @@ -1801,9 +1810,9 @@ start(bool external_bus) /* create the driver */ if (external_bus) { - g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG); + g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation); } else { - g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation); } if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -2015,13 +2024,17 @@ lsm303d_main(int argc, char *argv[]) { bool external_bus = false; int ch; + enum Rotation rotation = ROTATION_NONE; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "X")) != EOF) { + while ((ch = getopt(argc, argv, "XR:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; default: lsm303d_usage(); exit(0); @@ -2035,7 +2048,7 @@ lsm303d_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) - lsm303d::start(external_bus); + lsm303d::start(external_bus, rotation); /* * Test the driver/device. -- cgit v1.2.3 From a049f0841d0a68edf2f1e5d10ba2ab24d15aa472 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 14:10:38 +1000 Subject: Merged L3GD20 orientation flag while keeping original bus speed --- src/drivers/l3gd20/l3gd20.cpp | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 06b36d688..f8cd509a2 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -69,6 +69,7 @@ #include #include +#include #define L3GD20_DEVICE_PATH "/dev/l3gd20" @@ -185,7 +186,7 @@ extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI { public: - L3GD20(int bus, const char* path, spi_dev_e device); + L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation); virtual ~L3GD20(); virtual int init(); @@ -230,6 +231,8 @@ private: /* true if an L3G4200D is detected */ bool _is_l3g4200d; + enum Rotation _rotation; + /** * Start automatic measurement. */ @@ -329,7 +332,7 @@ private: int self_test(); }; -L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : +L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call_interval(0), _reports(nullptr), @@ -346,7 +349,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), - _is_l3g4200d(false) + _is_l3g4200d(false), + _rotation(rotation) { // enable debug() calls _debug_enabled = true; @@ -915,6 +919,9 @@ L3GD20::measure() report.y = _gyro_filter_y.apply(report.y); report.z = _gyro_filter_z.apply(report.z); + // apply user specified rotation + rotate_3f(_rotation, report.x, report.y, report.z); + report.scaling = _gyro_range_scale; report.range_rad_s = _gyro_range_rad_s; @@ -975,7 +982,7 @@ namespace l3gd20 L3GD20 *g_dev; -void start(); +void start(bool external_bus, enum Rotation rotation); void test(); void reset(); void info(); @@ -984,7 +991,7 @@ void info(); * Start the driver. */ void -start(bool external_bus) +start(bool external_bus, enum Rotation rotation) { int fd; @@ -994,12 +1001,12 @@ start(bool external_bus) /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO); + g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO, rotation); #else errx(0, "External SPI not available"); #endif } else { - g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO, rotation); } if (g_dev == nullptr) @@ -1128,13 +1135,17 @@ l3gd20_main(int argc, char *argv[]) { bool external_bus = false; int ch; + enum Rotation rotation = ROTATION_NONE; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "X")) != EOF) { + while ((ch = getopt(argc, argv, "XR:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; default: l3gd20_usage(); exit(0); @@ -1148,7 +1159,7 @@ l3gd20_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) - l3gd20::start(external_bus); + l3gd20::start(external_bus, rotation); /* * Test the driver/device. -- cgit v1.2.3 From d2487e771884d62f893b14629caae8437bd6998e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jul 2014 11:20:14 +1000 Subject: hmc5883: added support for -R rotation option --- src/drivers/hmc5883/hmc5883.cpp | 45 +++++++++++++++++++++++++++++++++-------- 1 file changed, 37 insertions(+), 8 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index b7b368a5e..75acf32e3 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -71,6 +71,8 @@ #include #include +#include +#include /* * HMC5883 internal constants and data structures. @@ -130,7 +132,7 @@ static const int ERROR = -1; class HMC5883 : public device::I2C { public: - HMC5883(int bus); + HMC5883(int bus, enum Rotation rotation); virtual ~HMC5883(); virtual int init(); @@ -169,6 +171,7 @@ private: bool _calibrated; /**< the calibration is valid */ int _bus; /**< the bus the device is connected to */ + enum Rotation _rotation; struct mag_report _last_report; /**< used for info() */ @@ -319,7 +322,7 @@ private: extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); -HMC5883::HMC5883(int bus) : +HMC5883::HMC5883(int bus, enum Rotation rotation) : I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), _reports(nullptr), @@ -334,7 +337,8 @@ HMC5883::HMC5883(int bus) : _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), _sensor_ok(false), _calibrated(false), - _bus(bus) + _bus(bus), + _rotation(rotation) { // enable debug() calls _debug_enabled = false; @@ -862,6 +866,9 @@ HMC5883::collect() /* z remains z */ new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + // apply user specified rotation + rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { if (_mag_topic != -1) { @@ -1246,7 +1253,7 @@ const int ERROR = -1; HMC5883 *g_dev; -void start(); +void start(enum Rotation rotation); void test(); void reset(); void info(); @@ -1256,7 +1263,7 @@ int calibrate(); * Start the driver. */ void -start() +start(enum Rotation rotation) { int fd; @@ -1265,7 +1272,7 @@ start() errx(0, "already started"); /* create the driver, attempt expansion bus first */ - g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION); + g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION, rotation); if (g_dev != nullptr && OK != g_dev->init()) { delete g_dev; g_dev = nullptr; @@ -1275,7 +1282,7 @@ start() #ifdef PX4_I2C_BUS_ONBOARD /* if this failed, attempt onboard sensor */ if (g_dev == nullptr) { - g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD); + g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD, rotation); if (g_dev != nullptr && OK != g_dev->init()) { goto fail; } @@ -1474,14 +1481,36 @@ info() } // namespace +void +hmc5883_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'"); + warnx("options:"); + warnx(" -R rotation"); +} + int hmc5883_main(int argc, char *argv[]) { + int ch; + enum Rotation rotation = ROTATION_NONE; + + while ((ch = getopt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; + default: + hmc5883_usage(); + exit(0); + } + } + /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) - hmc5883::start(); + hmc5883::start(rotation); /* * Test the driver/device. -- cgit v1.2.3 From d952e81ab7c5ef050784cf3766c5b7bf18909777 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jul 2014 11:48:44 +1000 Subject: added support for two instances of hmc5883 driver --- src/drivers/hmc5883/hmc5883.cpp | 115 +++++++++++++++++++++++++--------------- 1 file changed, 72 insertions(+), 43 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 75acf32e3..b9270d2a0 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -79,7 +79,8 @@ */ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 -#define HMC5883L_DEVICE_PATH "/dev/hmc5883" +#define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int" +#define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext" /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ @@ -132,7 +133,7 @@ static const int ERROR = -1; class HMC5883 : public device::I2C { public: - HMC5883(int bus, enum Rotation rotation); + HMC5883(int bus, const char *path, enum Rotation rotation); virtual ~HMC5883(); virtual int init(); @@ -322,8 +323,8 @@ private: extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); -HMC5883::HMC5883(int bus, enum Rotation rotation) : - I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), +HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : + I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ @@ -1251,13 +1252,14 @@ namespace hmc5883 #endif const int ERROR = -1; -HMC5883 *g_dev; +HMC5883 *g_dev_int; +HMC5883 *g_dev_ext; -void start(enum Rotation rotation); -void test(); -void reset(); +void start(int bus, enum Rotation rotation); +void test(int bus); +void reset(int bus); void info(); -int calibrate(); +int calibrate(int bus); /** * Start the driver. @@ -1267,47 +1269,69 @@ start(enum Rotation rotation) { int fd; - if (g_dev != nullptr) - /* if already started, the still command succeeded */ - errx(0, "already started"); - /* create the driver, attempt expansion bus first */ - g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION, rotation); - if (g_dev != nullptr && OK != g_dev->init()) { - delete g_dev; - g_dev = nullptr; + if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { + if (g_dev_ext != nullptr) + errx(0, "already started external"); + g_dev_ext = new HMC5883(PX4_I2C_BUS_EXPANSION, HMC5883L_DEVICE_PATH_EXT, rotation); + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { + delete g_dev_ext; + g_dev_ext = nullptr; + } } #ifdef PX4_I2C_BUS_ONBOARD /* if this failed, attempt onboard sensor */ - if (g_dev == nullptr) { - g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD, rotation); - if (g_dev != nullptr && OK != g_dev->init()) { + if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { + if (g_dev_int != nullptr) + errx(0, "already started internal"); + g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation); + if (g_dev_int != nullptr && OK != g_dev_int->init()) { + if (bus == PX4_I2C_BUS_ONBOARD) { + goto fail; + } + } + if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { goto fail; } } #endif - if (g_dev == nullptr) + if (g_dev_int == nullptr && g_dev_ext == nullptr) goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); + if (g_dev_int != nullptr) { + fd = open(HMC5883L_DEVICE_PATH_INT, O_RDONLY); + if (fd < 0) + goto fail; - if (fd < 0) - goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + close(fd); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; + if (g_dev_ext != nullptr) { + fd = open(HMC5883L_DEVICE_PATH_EXT, O_RDONLY); + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + close(fd); + } exit(0); fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + if (g_dev_int != nullptr) { + delete g_dev_int; + g_dev_int = nullptr; + } + if (g_dev_ext != nullptr) { + delete g_dev_ext; + g_dev_ext = nullptr; } errx(1, "driver start failed"); @@ -1319,16 +1343,17 @@ fail: * and automatic modes. */ void -test() +test(int bus) { struct mag_report report; ssize_t sz; int ret; + const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); - int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); + int fd = open(path, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -1421,14 +1446,15 @@ test() * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10. * Using the self test method described above, the user can scale sensor */ -int calibrate() +int calibrate(int bus) { int ret; + const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); - int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); + int fd = open(path, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); @@ -1448,9 +1474,11 @@ int calibrate() * Reset the driver. */ void -reset() +reset(int bus) { - int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); + const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + + int fd = open(path, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1468,8 +1496,9 @@ reset() * Print a little info about the driver. */ void -info() +info(int bus) { + HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); if (g_dev == nullptr) errx(1, "driver not running"); @@ -1516,25 +1545,25 @@ hmc5883_main(int argc, char *argv[]) * Test the driver/device. */ if (!strcmp(argv[1], "test")) - hmc5883::test(); + hmc5883::test(bus); /* * Reset the driver. */ if (!strcmp(argv[1], "reset")) - hmc5883::reset(); + hmc5883::reset(bus); /* * Print driver information. */ if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) - hmc5883::info(); + hmc5883::info(bus); /* * Autocalibrate the scaling */ if (!strcmp(argv[1], "calibrate")) { - if (hmc5883::calibrate() == 0) { + if (hmc5883::calibrate(bus) == 0) { errx(0, "calibration successful"); } else { -- cgit v1.2.3 From 5e62ae7a9e2a7d3ea05d293900f7171884fbb448 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jul 2014 12:07:23 +1000 Subject: hmc5883: added -C option to calibrate on startup Conflicts: src/drivers/hmc5883/hmc5883.cpp --- src/drivers/hmc5883/hmc5883.cpp | 42 ++++++++++++++++++++++++++++++++++------- 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index b9270d2a0..65c8c64d0 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1516,6 +1516,11 @@ hmc5883_usage() warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'"); warnx("options:"); warnx(" -R rotation"); + warnx(" -C calibrate on start"); + warnx(" -X only external bus"); +#ifdef PX4_I2C_BUS_ONBOARD + warnx(" -I only internal bus"); +#endif } int @@ -1523,46 +1528,69 @@ hmc5883_main(int argc, char *argv[]) { int ch; enum Rotation rotation = ROTATION_NONE; + bool calibrate = false; - while ((ch = getopt(argc, argv, "R:")) != EOF) { + while ((ch = getopt(argc, argv, "XIR:C")) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(optarg); break; +#ifdef PX4_I2C_BUS_ONBOARD + case 'I': + bus = PX4_I2C_BUS_ONBOARD; + break; +#endif + case 'X': + bus = PX4_I2C_BUS_EXPANSION; + break; + case 'C': + calibrate = true; + break; default: hmc5883_usage(); exit(0); } } + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - hmc5883::start(rotation); + if (!strcmp(verb, "start")) { + hmc5883::start(bus, rotation); + if (calibrate) { + if (hmc5883::calibrate(bus) == 0) { + errx(0, "calibration successful"); + + } else { + errx(1, "calibration failed"); + } + } + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) hmc5883::test(bus); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) hmc5883::reset(bus); /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(verb, "info") || !strcmp(verb, "status")) hmc5883::info(bus); /* * Autocalibrate the scaling */ - if (!strcmp(argv[1], "calibrate")) { + if (!strcmp(verb, "calibrate")) { if (hmc5883::calibrate(bus) == 0) { errx(0, "calibration successful"); -- cgit v1.2.3 From dfee93f3b128a7f23d74363b7700c80ababbe690 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 6 Jul 2014 22:37:40 +1000 Subject: hmc5883: fixed driver startup when trying both buses --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 65c8c64d0..25cbc0679 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1325,11 +1325,11 @@ start(enum Rotation rotation) exit(0); fail: - if (g_dev_int != nullptr) { + if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { delete g_dev_int; g_dev_int = nullptr; } - if (g_dev_ext != nullptr) { + if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { delete g_dev_ext; g_dev_ext = nullptr; } -- cgit v1.2.3 From 1c6ea067902708a2b1b3faf55938e6e8768abe18 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Jul 2014 08:52:16 +1000 Subject: hmc5883: fixed build warnings --- src/drivers/hmc5883/hmc5883.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 25cbc0679..72d6bdd95 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1255,10 +1255,11 @@ const int ERROR = -1; HMC5883 *g_dev_int; HMC5883 *g_dev_ext; +void hmc5883_usage(); void start(int bus, enum Rotation rotation); void test(int bus); void reset(int bus); -void info(); +void info(int bus); int calibrate(int bus); /** -- cgit v1.2.3 From 42716345859ded979189ea7f2548d512975c4dec Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Jul 2014 08:52:27 +1000 Subject: l3gd20: fixed a build warning --- src/drivers/l3gd20/l3gd20.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index f8cd509a2..cad3ff937 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -982,6 +982,7 @@ namespace l3gd20 L3GD20 *g_dev; +void l3gd20_usage(); void start(bool external_bus, enum Rotation rotation); void test(); void reset(); -- cgit v1.2.3 From 1b555f2d2e0684485fa47db7acfcf5555a7c6b16 Mon Sep 17 00:00:00 2001 From: akdslr Date: Tue, 15 Apr 2014 15:28:30 -0400 Subject: LL40LS driver: Added new driver to the config make files --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + 2 files changed, 2 insertions(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 4d727aa4d..cc0958c29 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -25,6 +25,7 @@ MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx +MODULES += drivers/ll40ls MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 8e83df391..adfbc2b7d 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -28,6 +28,7 @@ MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/sf0x +MODULES += drivers/ll40ls MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry -- cgit v1.2.3 From 36a9123822dca314f4efc4b4cb140159dbe9e634 Mon Sep 17 00:00:00 2001 From: akdslr Date: Tue, 15 Apr 2014 20:50:31 -0400 Subject: LL40LS driver: adding new range finder driver --- src/drivers/ll40ls/ll40ls.cpp | 880 ++++++++++++++++++++++++++++++++++++++++++ src/drivers/ll40ls/module.mk | 40 ++ 2 files changed, 920 insertions(+) create mode 100644 src/drivers/ll40ls/ll40ls.cpp create mode 100644 src/drivers/ll40ls/module.mk diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp new file mode 100644 index 000000000..cb0eac91b --- /dev/null +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -0,0 +1,880 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ll40ls.cpp + * @author Allyson Kreft + * + * Driver for the PulsedLight Lidar-Lite range finders connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +/* Configuration Constants */ +#define LL40LS_BUS PX4_I2C_BUS_EXPANSION +#define LL40LS_BASEADDR 0x42 /* 7-bit address */ +#define LL40LS_DEVICE_PATH "/dev/ll40ls" + +/* LL40LS Registers addresses */ + +#define LL40LS_MEASURE_REG 0x00 /* Measure range register */ +#define LL40LS_MSRREG_ACQUIRE 0x61 /* Value to initiate a measurement */ +#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ + +/* Device limits */ +#define LL40LS_MIN_DISTANCE (0.00f) +#define LL40LS_MAX_DISTANCE (14.00f) + +#define LL40LS_CONVERSION_INTERVAL 60000 /* 60ms */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class LL40LS : public device::I2C +{ +public: + LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR); + virtual ~LL40LS(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE + * and LL40LS_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]); + +LL40LS::LL40LS(int bus, int address) : + I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000), + _min_distance(LL40LS_MIN_DISTANCE), + _max_distance(LL40LS_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _range_finder_topic(-1), + _class_instance(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), + _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")) +{ + // up the retries since the device misses the first measure attempts + I2C::_retries = 3; + + // enable debug() calls + _debug_enabled = false; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +LL40LS::~LL40LS() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); +} + +int +LL40LS::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + goto out; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + + if (_reports == nullptr) { + goto out; + } + + _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +LL40LS::probe() +{ + return measure(); +} + +void +LL40LS::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +LL40LS::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +LL40LS::get_minimum_distance() +{ + return _min_distance; +} + +float +LL40LS::get_maximum_distance() +{ + return _max_distance; +} + +int +LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +LL40LS::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(LL40LS_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +LL40LS::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; + ret = transfer(cmd, sizeof(cmd), nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +LL40LS::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + // read the high and low byte distance registers + uint8_t distance_reg = LL40LS_DISTHIGH_REG; + ret = transfer(&distance_reg, 1, &val[0], sizeof(val)); + + if (ret < 0) { + log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint16_t distance = val[0] << 8 | val[1]; + float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ + struct range_finder_report report; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { + report.valid = 1; + } + else { + report.valid = 0; + } + + /* publish it */ + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +LL40LS::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&LL40LS::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +LL40LS::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +LL40LS::cycle_trampoline(void *arg) +{ + LL40LS *dev = (LL40LS *)arg; + + dev->cycle(); +} + +void +LL40LS::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&LL40LS::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + log("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&LL40LS::cycle_trampoline, + this, + USEC2TICK(LL40LS_CONVERSION_INTERVAL)); +} + +void +LL40LS::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace ll40ls +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +LL40LS *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new LL40LS(LL40LS_BUS); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +ll40ls_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + ll40ls::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + ll40ls::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + ll40ls::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + ll40ls::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + ll40ls::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/ll40ls/module.mk b/src/drivers/ll40ls/module.mk new file mode 100644 index 000000000..ab7d43269 --- /dev/null +++ b/src/drivers/ll40ls/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the PulsedLight Lidar-Lite driver. +# + +MODULE_COMMAND = ll40ls + +SRCS = ll40ls.cpp -- cgit v1.2.3 From 7f91f0cc3ef806a7959e06ab16d4094bcfe871bd Mon Sep 17 00:00:00 2001 From: akdslr Date: Wed, 30 Apr 2014 14:09:30 -0400 Subject: LL40LS driver: Updated the value to write to the register to trigger a measurement --- src/drivers/ll40ls/ll40ls.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index cb0eac91b..287207837 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -79,7 +79,7 @@ /* LL40LS Registers addresses */ #define LL40LS_MEASURE_REG 0x00 /* Measure range register */ -#define LL40LS_MSRREG_ACQUIRE 0x61 /* Value to initiate a measurement */ +#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ #define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ /* Device limits */ -- cgit v1.2.3 From 67b7a858881229b95483e986a6e4d90ab34ac257 Mon Sep 17 00:00:00 2001 From: akdslr Date: Thu, 5 Jun 2014 09:53:56 -0400 Subject: LL40LS driver: Changes from the May 4th plane test flight --- src/drivers/ll40ls/ll40ls.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 287207837..bc012e2e9 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -86,7 +86,7 @@ #define LL40LS_MIN_DISTANCE (0.00f) #define LL40LS_MAX_DISTANCE (14.00f) -#define LL40LS_CONVERSION_INTERVAL 60000 /* 60ms */ +#define LL40LS_CONVERSION_INTERVAL 100000 /* 100ms */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -517,8 +517,8 @@ LL40LS::collect() return ret; } - uint16_t distance = val[0] << 8 | val[1]; - float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ + uint16_t distance = (val[0] << 8) | val[1]; + float si_units = distance * 0.01f; /* cm to m */ struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ @@ -532,8 +532,10 @@ LL40LS::collect() report.valid = 0; } - /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } if (_reports->force(&report)) { perf_count(_buffer_overflows); -- cgit v1.2.3 From ba54983cdf33b55b47f3b46c9b2461ab7217461a Mon Sep 17 00:00:00 2001 From: akdslr Date: Fri, 6 Jun 2014 08:52:06 -0400 Subject: MB12XX Driver: Added a class instance and device specific path --- src/drivers/mb12xx/mb12xx.cpp | 45 ++++++++++++++++++++++++++++++------------- 1 file changed, 32 insertions(+), 13 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 5adb8cf69..b63e54dac 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -74,6 +74,7 @@ /* Configuration Constants */ #define MB12XX_BUS PX4_I2C_BUS_EXPANSION #define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ +#define MB12XX_DEVICE_PATH "/dev/mb12xx" /* MB12xx Registers addresses */ @@ -124,6 +125,7 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; + int _class_instance; orb_advert_t _range_finder_topic; @@ -187,7 +189,7 @@ private: extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]); MB12XX::MB12XX(int bus, int address) : - I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), + I2C("MB12xx", MB12XX_DEVICE_PATH, bus, address, 100000), _min_distance(MB12XX_MIN_DISTANCE), _max_distance(MB12XX_MAX_DISTANCE), _reports(nullptr), @@ -195,6 +197,7 @@ MB12XX::MB12XX(int bus, int address) : _measure_ticks(0), _collect_phase(false), _range_finder_topic(-1), + _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) @@ -215,6 +218,15 @@ MB12XX::~MB12XX() if (_reports != nullptr) { delete _reports; } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int @@ -234,13 +246,18 @@ MB12XX::init() goto out; } - /* get a publish handle on the range finder topic */ - struct range_finder_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); - if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } } ret = OK; @@ -505,8 +522,10 @@ MB12XX::collect() report.distance = si_units; report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; - /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } if (_reports->force(&report)) { perf_count(_buffer_overflows); @@ -665,7 +684,7 @@ start() } /* set the poll rate to default, starts automatic data collection */ - fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + fd = open(MB12XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { goto fail; @@ -715,10 +734,10 @@ test() ssize_t sz; int ret; - int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + int fd = open(MB12XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + err(1, "%s open failed (try 'mb12xx start' if the driver is not running", MB12XX_DEVICE_PATH); } /* do a simple demand read */ @@ -776,7 +795,7 @@ test() void reset() { - int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + int fd = open(MB12XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { err(1, "failed "); -- cgit v1.2.3 From 681d746e060cbd82008967ea56dc950c9ca7ef49 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 13:21:08 +0200 Subject: update mavlink to latest master --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 3711190d2..d1ebe85eb 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 3711190d23d9928ea0687e00621c8d9ecf145f50 +Subproject commit d1ebe85eb6bb06d0078f3e0b8144adb131263628 -- cgit v1.2.3 From a6b52f1c9f720a8dd31099e7adeedee694f60729 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:49:24 +0200 Subject: HMC5883 post merge cleanup --- src/drivers/device/spi.cpp | 4 ++-- src/drivers/hmc5883/hmc5883.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index fed6c312c..0b232e158 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -69,11 +69,11 @@ SPI::SPI(const char *name, // protected locking_mode(LOCK_PREEMPTION), // private - _bus(bus), _device(device), _mode(mode), _frequency(frequency), - _dev(nullptr) + _dev(nullptr), + _bus(bus) { } diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 72d6bdd95..c2e4700d8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1266,7 +1266,7 @@ int calibrate(int bus); * Start the driver. */ void -start(enum Rotation rotation) +start(int bus, enum Rotation rotation) { int fd; @@ -1528,6 +1528,7 @@ int hmc5883_main(int argc, char *argv[]) { int ch; + int bus = -1; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; -- cgit v1.2.3 From 369c7d277f2fea351ca4243debccc0a115f3f7e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:50:00 +0200 Subject: Board config cleanup for external bus support --- makefiles/config_px4fmu-v2_test.mk | 1 + src/drivers/boards/px4fmu-v1/board_config.h | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 66d9efbf8..395a8f2ac 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -42,6 +42,7 @@ MODULES += modules/uORB LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter +MODULES += lib/conversion # # Libraries diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index c944007a5..a70a6240c 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -86,7 +86,6 @@ __BEGIN_DECLS #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) #define PX4_SPI_BUS_SENSORS 1 -#define PX4_SPI_BUS_EXT 2 /* * Use these in place of the spi_dev_e enumeration to -- cgit v1.2.3 From a42ec7df1b417a34e072a68c6e34240a97d5ba80 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:50:24 +0200 Subject: MS5611: Deal with missing external bus --- src/drivers/ms5611/ms5611_spi.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 00d016aed..5234ce8d6 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -118,7 +118,11 @@ device::Device * MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) { if (external_bus) { + #ifdef PX4_SPI_BUS_EXT return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); + #else + return nullptr; + #endif } else { return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); } -- cgit v1.2.3 From 92426c5cfc8e9746fe42e4c9c087a5e25a4be658 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:51:33 +0200 Subject: LSM303D: deal with missing external bus --- src/drivers/lsm303d/lsm303d.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index f25ac8f87..296899ccd 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1810,10 +1810,15 @@ start(bool external_bus, enum Rotation rotation) /* create the driver */ if (external_bus) { + #ifdef PX4_SPI_BUS_EXT g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation); + #else + errx(0, "External SPI not available"); + #endif } else { g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation); } + if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); goto fail; -- cgit v1.2.3 From 7d15e999f12fcfa7650c673e8be1daffbbfc46d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:52:10 +0200 Subject: LSM303: Fix usage function call to fit existing structure. --- src/drivers/lsm303d/lsm303d.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 296899ccd..7ba0fcc88 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1797,6 +1797,7 @@ void reset(); void info(); void regdump(); void logging(); +void usage(); /** * Start the driver. @@ -2013,17 +2014,17 @@ logging() exit(0); } - -} // namespace - void -lsm303d_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -R rotation"); } +} // namespace + int lsm303d_main(int argc, char *argv[]) { @@ -2041,7 +2042,7 @@ lsm303d_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(optarg); break; default: - lsm303d_usage(); + lsm303d::usage(); exit(0); } } -- cgit v1.2.3 From 875be65242dc58503c44ff522372b3cc2df83619 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:52:28 +0200 Subject: MPU6000: Fix usage function call to fit existing structure. --- src/drivers/mpu6000/mpu6000.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index f6e00feee..5668b0865 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1433,6 +1433,7 @@ void start(bool, enum Rotation); void test(bool); void reset(bool); void info(bool); +void usage(); /** * Start the driver. @@ -1604,17 +1605,17 @@ info(bool external_bus) exit(0); } - -} // namespace - void -mpu6000_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -R rotation"); } +} // namespace + int mpu6000_main(int argc, char *argv[]) { @@ -1632,7 +1633,7 @@ mpu6000_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(optarg); break; default: - mpu6000_usage(); + mpu6000::usage(); exit(0); } } -- cgit v1.2.3 From 812d326912feb41bb37848d61b1a6e6e1d18ab90 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:52:42 +0200 Subject: L3GD20: Fix usage function call to fit existing structure. --- src/drivers/l3gd20/l3gd20.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index cad3ff937..64d1a7e55 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -982,7 +982,7 @@ namespace l3gd20 L3GD20 *g_dev; -void l3gd20_usage(); +void usage(); void start(bool external_bus, enum Rotation rotation); void test(); void reset(); @@ -1120,17 +1120,17 @@ info() exit(0); } - -} // namespace - void -l3gd20_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -R rotation"); } +} // namespace + int l3gd20_main(int argc, char *argv[]) { @@ -1148,7 +1148,7 @@ l3gd20_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(optarg); break; default: - l3gd20_usage(); + l3gd20::usage(); exit(0); } } -- cgit v1.2.3 From f2cbc7fe2778b4148bdecd59148ec96b84a248df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:53:10 +0200 Subject: MB12xx: Fix initialiser order in class --- src/drivers/mb12xx/mb12xx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index b63e54dac..beb6c8e35 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -196,8 +196,8 @@ MB12XX::MB12XX(int bus, int address) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), - _range_finder_topic(-1), _class_instance(-1), + _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) -- cgit v1.2.3 From 83f343f196c07d0a633ee8fc7d2324e8cb3d129c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:53:25 +0200 Subject: LL40LS: Fix initialiser order in class --- src/drivers/ll40ls/ll40ls.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index bc012e2e9..a69e6ee55 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -196,8 +196,8 @@ LL40LS::LL40LS(int bus, int address) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), - _range_finder_topic(-1), _class_instance(-1), + _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")) -- cgit v1.2.3 From ea4de4adc388e9353114e6e2c89eb27cec5b249e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:54:01 +0200 Subject: param command: fix warnings --- src/systemcmds/param/param.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 28e1b108b..e110335e7 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,7 +63,7 @@ static void do_import(const char* param_file_name); static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val, bool fail_on_not_found); -static void do_compare(const char* name, const char* vals[], unsigned comparisons); +static void do_compare(const char* name, char* vals[], unsigned comparisons); static void do_reset(void); static void do_reset_nostart(void); @@ -351,7 +351,7 @@ do_set(const char* name, const char* val, bool fail_on_not_found) } static void -do_compare(const char* name, const char* vals[], unsigned comparisons) +do_compare(const char* name, char* vals[], unsigned comparisons) { int32_t i; float f; -- cgit v1.2.3 From e380aa3f62c9db89f048f3683e3611e455de4ad2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:54:33 +0200 Subject: AT24C: Fix warning due to missing function prototype. --- src/systemcmds/mtd/24xxxx_mtd.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index 72200f418..991363797 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -161,6 +161,7 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); void at24c_test(void); +int at24c_nuke(void); /************************************************************************************ * Private Data -- cgit v1.2.3 From ac8f179f2dad99d664a6f9de4df954bea7fe5858 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 20:24:18 +1000 Subject: Tools: skip check_submodules.sh when NUTTX_SRC is set this avoids using submodules when a specific NuttX tree is specified --- Tools/check_submodules.sh | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index fb180ef47..4b8789b28 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -1,5 +1,12 @@ #!/bin/sh +[ -n "$NUTTX_SRC" ] && { + # NUTTX_SRC is set, meaning user is overriding the NuttX tree to use. Don't + # use submodules to pull an alternatiie tree + echo "Skipping submodules as NUTTX_SRC is set to $NUTTX_SRC" + exit 0 +} + if [ -d NuttX/nuttx ]; then STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") -- cgit v1.2.3 From 52713bc0ba891854934f62ad856f1600e8ba3065 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 15:00:44 +0200 Subject: Revert "Tools: skip check_submodules.sh when NUTTX_SRC is set" This reverts commit ac8f179f2dad99d664a6f9de4df954bea7fe5858. --- Tools/check_submodules.sh | 7 ------- 1 file changed, 7 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 4b8789b28..fb180ef47 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -1,12 +1,5 @@ #!/bin/sh -[ -n "$NUTTX_SRC" ] && { - # NUTTX_SRC is set, meaning user is overriding the NuttX tree to use. Don't - # use submodules to pull an alternatiie tree - echo "Skipping submodules as NUTTX_SRC is set to $NUTTX_SRC" - exit 0 -} - if [ -d NuttX/nuttx ]; then STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") -- cgit v1.2.3 From bc06d839eab504e039c27cd472bbd1162cb7bbf4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 20:24:18 +1000 Subject: Tools: skip check_submodules.sh when GIT_SUBMODULES_ARE_EVIL is set this avoids using submodules when a specific NuttX tree is specified --- Tools/check_submodules.sh | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index fb180ef47..55bcc05ee 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -1,5 +1,11 @@ #!/bin/sh +[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && { + # GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules + echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC" + exit 0 +} + if [ -d NuttX/nuttx ]; then STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") -- cgit v1.2.3 From 76f82bf237de367a3a3b214f2ba06faddb077c57 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 15:04:01 +0200 Subject: Updated submodule update instructions --- Tools/check_submodules.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 55bcc05ee..7c3a3bfc2 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -12,7 +12,7 @@ if [ -d NuttX/nuttx ]; if [ "$STATUSRETVAL" == "" ]; then echo "Checked NuttX submodule, correct version found" else - echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'" + echo "NuttX sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi @@ -28,7 +28,7 @@ if [ -d mavlink/include/mavlink/v1.0 ]; if [ "$STATUSRETVAL" == "" ]; then echo "Checked mavlink submodule, correct version found" else - echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'" + echo "mavlink sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi -- cgit v1.2.3 From ee9233451244604f1522dda5e58d1deb7ec6473d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 20:23:07 +1000 Subject: build: fixed running build from external directory --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 8bf96ca23..7b7c00704 100644 --- a/Makefile +++ b/Makefile @@ -210,11 +210,11 @@ menuconfig: endif $(NUTTX_SRC): - $(Q) (./Tools/check_submodules.sh) + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) .PHONY: checksubmodules checksubmodules: - $(Q) (./Tools/check_submodules.sh) + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) .PHONY: updatesubmodules updatesubmodules: -- cgit v1.2.3 From 0054eb23d857844ebae34a7d198fc60e538ccd3c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 15:44:22 +0200 Subject: Update sensors tests --- src/systemcmds/tests/test_sensors.c | 37 ++++++++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index 096106ff3..a4f17eebd 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -139,7 +139,14 @@ accel(int argc, char *argv[]) } if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { - warnx("ACCEL1 acceleration values out of range!"); + warnx("ACCEL acceleration values out of range!"); + return ERROR; + } + + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len < 8.0f || len > 12.0f) { + warnx("ACCEL scale error!"); return ERROR; } @@ -186,6 +193,13 @@ accel1(int argc, char *argv[]) return ERROR; } + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len < 8.0f || len > 12.0f) { + warnx("ACCEL1 scale error!"); + return ERROR; + } + /* Let user know everything is ok */ printf("\tOK: ACCEL1 passed all tests successfully\n"); @@ -225,6 +239,13 @@ gyro(int argc, char *argv[]) printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); } + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len > 0.3f) { + warnx("GYRO scale error!"); + return ERROR; + } + /* Let user know everything is ok */ printf("\tOK: GYRO passed all tests successfully\n"); close(fd); @@ -263,6 +284,13 @@ gyro1(int argc, char *argv[]) printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); } + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len > 0.3f) { + warnx("GYRO1 scale error!"); + return ERROR; + } + /* Let user know everything is ok */ printf("\tOK: GYRO1 passed all tests successfully\n"); close(fd); @@ -301,6 +329,13 @@ mag(int argc, char *argv[]) printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z); } + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len < 1.0f || len > 3.0f) { + warnx("MAG scale error!"); + return ERROR; + } + /* Let user know everything is ok */ printf("\tOK: MAG passed all tests successfully\n"); close(fd); -- cgit v1.2.3 From b4ab31a2bac2b069528d56740901b3d69d2f2227 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 8 Jul 2014 19:11:28 +0200 Subject: A more compatible way of checking for an empty string. --- Tools/check_submodules.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index fb180ef47..086b6f35d 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -3,7 +3,7 @@ if [ -d NuttX/nuttx ]; then STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") - if [ "$STATUSRETVAL" == "" ]; then + if [ -z $STATUSRETVAL ]; then echo "Checked NuttX submodule, correct version found" else echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'" @@ -19,7 +19,7 @@ fi if [ -d mavlink/include/mavlink/v1.0 ]; then STATUSRETVAL=$(git status --porcelain | grep -i "mavlink/include/mavlink/v1.0") - if [ "$STATUSRETVAL" == "" ]; then + if [ -z $STATUSRETVAL ]; then echo "Checked mavlink submodule, correct version found" else echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'" -- cgit v1.2.3 From 13b41a2629ddf964951eb228534c5a9680a0f100 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 8 Jul 2014 19:17:25 +0200 Subject: Add quotes around the variable. It's safer. --- Tools/check_submodules.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 086b6f35d..db81ed04a 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -3,7 +3,7 @@ if [ -d NuttX/nuttx ]; then STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") - if [ -z $STATUSRETVAL ]; then + if [ -z "$STATUSRETVAL" ]; then echo "Checked NuttX submodule, correct version found" else echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'" @@ -19,7 +19,7 @@ fi if [ -d mavlink/include/mavlink/v1.0 ]; then STATUSRETVAL=$(git status --porcelain | grep -i "mavlink/include/mavlink/v1.0") - if [ -z $STATUSRETVAL ]; then + if [ -z "$STATUSRETVAL" ]; then echo "Checked mavlink submodule, correct version found" else echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'" -- cgit v1.2.3 From c39e572a063b8e17c2c8f896fdae4d0ec159960f Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 8 Jul 2014 23:58:16 +0200 Subject: Updated the Wing Wing params based on latest flight tests. --- ROMFS/px4fmu_common/init.d/3033_wingwing | 27 +++++++++++++++++++-------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index fff4b58df..765685ccc 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -11,25 +11,36 @@ if [ $DO_AUTOCONFIG == yes ] then param set BAT_N_CELLS 2 param set FW_AIRSPD_MAX 15 - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 11 + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 13 param set FW_ATT_TC 0.3 param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 12 + param set FW_L1_PERIOD 16 param set FW_LND_ANG 15 param set FW_LND_FLALT 5 param set FW_LND_HHDIST 15 param set FW_LND_HVIRT 13 param set FW_LND_TLALT 5 param set FW_THR_LND_MAX 0 - param set FW_P_ROLLFF 2 - param set FW_PR_FF 0.6 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.06 + param set FW_PR_FF 0.35 + param set FW_PR_I 0.005 + param set FW_PR_IMAX 0.4 + param set FW_PR_P 0.08 param set FW_RR_FF 0.6 + param set FW_RR_I 0.005 param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.09 + param set FW_RR_P 0.04 param set FW_THR_CRUISE 0.65 + param set MT_TKF_PIT_MAX 30.0 + param set MT_ACC_D 0.2 + param set MT_ACC_P 0.6 + param set MT_A_LP 0.5 + param set MT_PIT_OFF 0.1 + param set MT_PIT_I 0.1 + param set MT_THR_OFF 0.12 + param set MT_THR_I 0.35 + param set MT_THR_P 0.2 + param set MT_THR_FF 1.5 fi set MIXER FMU_Q -- cgit v1.2.3 From 8c6745d53fcb75c14caa21f3cb917ab6fa5f2bcf Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Thu, 10 Jul 2014 00:41:09 -0400 Subject: Prevent stack overflow when flashing px4io Large local variable causing stack overflow when attempting to flash IO!!! --- src/drivers/px4io/px4io_uploader.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 7b6361a7c..652949e41 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -413,11 +414,17 @@ static int read_with_retry(int fd, void *buf, size_t n) int PX4IO_Uploader::program(size_t fw_size) { - uint8_t file_buf[PROG_MULTI_MAX]; + uint8_t *file_buf; ssize_t count; int ret; size_t sent = 0; + file_buf = (uint8_t *)malloc(PROG_MULTI_MAX); + if (!file_buf) { + log("Can't allocate program buffer"); + return -ENOMEM; + } + log("programming %u bytes...", (unsigned)fw_size); ret = lseek(_fw_fd, 0, SEEK_SET); @@ -438,8 +445,10 @@ PX4IO_Uploader::program(size_t fw_size) (int)errno); } - if (count == 0) + if (count == 0) { + free(file_buf); return OK; + } sent += count; @@ -455,9 +464,12 @@ PX4IO_Uploader::program(size_t fw_size) ret = get_sync(1000); - if (ret != OK) + if (ret != OK) { + free(file_buf); return ret; + } } + free(file_buf); return OK; } -- cgit v1.2.3 From 12da0efbb24ad135dbacb4b8f90199b3b38cb40d Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Thu, 10 Jul 2014 00:55:33 -0400 Subject: Read the full buffer sizeof wont work here since file_buf is now a pointer --- src/drivers/px4io/px4io_uploader.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 652949e41..d134c0246 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -432,8 +432,8 @@ PX4IO_Uploader::program(size_t fw_size) while (sent < fw_size) { /* get more bytes to program */ size_t n = fw_size - sent; - if (n > sizeof(file_buf)) { - n = sizeof(file_buf); + if (n > PROG_MULTI_MAX) { + n = PROG_MULTI_MAX; } count = read_with_retry(_fw_fd, file_buf, n); -- cgit v1.2.3 From 9b2d444cc56eaaedcf271f200b93dcca94623209 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Jul 2014 14:08:09 +0200 Subject: dataman: use DM_KEY_WAYPOINTS_OFFBOARD() macro everywhere --- src/modules/mavlink/mavlink_mission.cpp | 4 ++-- src/modules/navigator/mission.cpp | 23 +++++------------------ 2 files changed, 7 insertions(+), 20 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index b2288469c..0dda2337e 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -216,7 +216,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ void MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) { - dm_item_t dm_item = _dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id); struct mission_item_s mission_item; if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { @@ -650,7 +650,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) return; } - dm_item_t dm_item = _transfer_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 53f0724cd..06e915f27 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -201,19 +201,11 @@ Mission::update_offboard_mission() /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ - dm_item_t dm_current; + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); - if (_offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, - (size_t)_offboard_mission.count, - _navigator->get_geofence(), - _navigator->get_home_position()->alt); + missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), + _navigator->get_home_position()->alt); } else { warnx("offboard mission update failed"); @@ -474,12 +466,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s mission = &_offboard_mission; - if (_offboard_mission.dataman_id == 0) { - dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_item = DM_KEY_WAYPOINTS_OFFBOARD_1; - } + dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { -- cgit v1.2.3 From bc602ff1e2c2ab6c08a7d5edf0b1f03308e011f8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Jul 2014 14:38:38 +0200 Subject: mavlink submodule fixed --- mavlink/include/mavlink/v1.0 | 1 + 1 file changed, 1 insertion(+) create mode 160000 mavlink/include/mavlink/v1.0 diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 new file mode 160000 index 000000000..d1ebe85eb --- /dev/null +++ b/mavlink/include/mavlink/v1.0 @@ -0,0 +1 @@ +Subproject commit d1ebe85eb6bb06d0078f3e0b8144adb131263628 -- cgit v1.2.3 From 3157b06fee99b57fe336f7772b293f8689ff8cdf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 15:52:19 +0200 Subject: Generalized the airspeed check --- src/modules/commander/state_machine_helper.cpp | 29 ++++++++++---------------- 1 file changed, 11 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6b96e3a3f..d894c9db0 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -666,29 +667,21 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { - fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) { + fd = orb_subscribe(ORB_ID(airspeed)); + + struct airspeed_s airspeed; + + if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) || + hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); - ret = fd; goto system_eval; } - struct differential_pressure_s diff_pres; - - ret = read(fd, &diff_pres, sizeof(diff_pres)); - - if (ret == sizeof(diff_pres)) { - if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { - mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); - // XXX do not make this fatal yet - ret = OK; - } - } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); - /* this is frickin' fatal */ - ret = ERROR; - goto system_eval; + if (fabsf(airspeed.indicated_airspeed_m_s > 5.0f)) { + mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + // XXX do not make this fatal yet + ret = OK; } } -- cgit v1.2.3 From 5bb8c501122de7daece58c5770b6aca13c0066cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 16:14:21 +0200 Subject: Fixed the submodule check logic --- Tools/check_submodules.sh | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index db81ed04a..a56de681f 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -2,12 +2,19 @@ if [ -d NuttX/nuttx ]; then - STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") + STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<") if [ -z "$STATUSRETVAL" ]; then echo "Checked NuttX submodule, correct version found" else - echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'" + echo "" + echo "" + echo "NuttX sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" exit 1 fi else @@ -18,12 +25,19 @@ fi if [ -d mavlink/include/mavlink/v1.0 ]; then - STATUSRETVAL=$(git status --porcelain | grep -i "mavlink/include/mavlink/v1.0") + STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<") if [ -z "$STATUSRETVAL" ]; then echo "Checked mavlink submodule, correct version found" else - echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'" + echo "" + echo "" + echo "mavlink sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" exit 1 fi else -- cgit v1.2.3 From e57579fa21ce9be189475e41fbcdb961d7cd32d2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Jul 2014 17:08:23 +0200 Subject: mavlink, navigator: compile errors/warnings fixed --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- src/modules/mavlink/mavlink_mission.cpp | 2 +- src/modules/navigator/mission.cpp | 12 ++++-------- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5c7eefa98..0d685c3d8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -226,8 +226,9 @@ Mavlink::Mavlink() : _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), - _mission_result_sub(-1), + _mission_manager(nullptr), _mission_pub(-1), + _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), _total_counter(0), _verbose(false), @@ -245,7 +246,6 @@ Mavlink::Mavlink() : _param_component_id(0), _param_system_type(0), _param_use_hil_gps(0), - _mission_manager(nullptr), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 0dda2337e..ca01344d4 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -69,9 +69,9 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), + _dataman_id(0), _count(0), _current_seq(0), - _dataman_id(0), _transfer_dataman_id(0), _transfer_count(0), _transfer_seq(0), diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index eb8dcc717..126877651 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -72,10 +72,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), - _dist_1wp_ok(false), - _need_takeoff(true), - _takeoff(false) - + _dist_1wp_ok(false) { /* load initial params */ updateParams(); @@ -203,7 +200,7 @@ Mission::update_offboard_mission() /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ - dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), @@ -252,13 +249,12 @@ Mission::check_dist_1wp() } /* check if first waypoint is not too far from home */ - bool mission_valid = false; if (_param_dist_1wp.get() > 0.0f) { if (_navigator->get_vstatus()->condition_home_position_valid) { struct mission_item_s mission_item; /* find first waypoint (with lat/lon) item in datamanager */ - for (int i = 0; i < _offboard_mission.count; i++) { + for (unsigned i = 0; i < _offboard_mission.count; i++) { if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { @@ -281,7 +277,7 @@ Mission::check_dist_1wp() } else { /* item is too far from home */ - mavlink_log_info(_navigator->get_mavlink_fd(), "first wp is too far from home: %.1fm > %.1fm", dist_to_1wp, _param_dist_1wp.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "first wp is too far from home: %.1fm > %.1fm", (double)dist_to_1wp, (double)_param_dist_1wp.get()); return false; } } -- cgit v1.2.3 From 687612dd654bcdfb390bfdddf8a004b10119c2ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 18:34:33 +0200 Subject: Do not abort if the sensor reset failed, only abort on no data --- src/modules/commander/airspeed_calibration.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 5d21d89d0..923c1a3ff 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -95,10 +95,8 @@ int do_airspeed_calibration(int mavlink_fd) } if (!paramreset_successful) { - warn("FAILED to set scale / offsets for airspeed"); - mavlink_log_critical(mavlink_fd, "dpress reset failed"); - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); - return ERROR; + warn("FAILED to reset - assuming analog"); + mavlink_log_critical(mavlink_fd, "assuming analog sensor"); } while (calibration_counter < calibration_count) { -- cgit v1.2.3 From dca8daeec54624001f02acb7ad6f4c2d7304a5fb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Jul 2014 21:37:00 +0200 Subject: mavlink: use all outputs in HIL mode --- src/modules/mavlink/mavlink_messages.cpp | 55 +++++++++++++------------------- 1 file changed, 23 insertions(+), 32 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 667be87b7..c7ad605c5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1051,10 +1051,16 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + /* scale outputs depending on system type */ if (mavlink_system.type == MAV_TYPE_QUADROTOR || mavlink_system.type == MAV_TYPE_HEXAROTOR || mavlink_system.type == MAV_TYPE_OCTOROTOR) { - /* set number of valid outputs depending on vehicle type */ + /* multirotors: set number of rotor outputs depending on type */ + unsigned n; switch (mavlink_system.type) { @@ -1071,59 +1077,44 @@ protected: break; } - /* scale / assign outputs depending on system type */ - float out[8]; - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (i < n) { + /* scale PWM out 900..2100 us to 0..1 for rotors */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); } else { - /* send 0 when disarmed */ - out[i] = 0.0f; + /* scale PWM out 900..2100 us to -1..1 for other channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); } } else { - out[i] = -1.0f; + /* send 0 when disarmed */ + out[i] = 0.0f; } } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); } else { - - /* fixed wing: scale all channels except throttle -1 .. 1 - * because we know that we set the mixers up this way - */ - - float out[8]; - - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) { if (i != 3) { - /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ + /* scale PWM out 900..2100 us to -1..1 for normal channels */ out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); } else { - - /* scale fake PWM out 900..2100 us to 0..1 for throttle */ + /* scale PWM out 900..2100 us to 0..1 for throttle */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); } } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } } }; -- cgit v1.2.3 From c97e08bcf05455e9e9b582fe3216250a988890fb Mon Sep 17 00:00:00 2001 From: hxxnrx Date: Thu, 10 Jul 2014 21:50:23 +0200 Subject: Set IO PX4_I2C_BUS_ONBOARD I2C speed to 400KHz --- src/drivers/px4io/px4io_i2c.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) mode change 100644 => 100755 src/drivers/px4io/px4io_i2c.cpp diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp old mode 100644 new mode 100755 index 19776c40a..c57ddf65b --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -79,7 +79,7 @@ device::Device } PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : - I2C("PX4IO_i2c", nullptr, bus, address, 320000) + I2C("PX4IO_i2c", nullptr, bus, address, 400000) { _retries = 3; } -- cgit v1.2.3 From 50414257a8802fe7c51a370f598899c8b554914c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:09:57 +0200 Subject: Remove voltage field for digital sensors --- src/drivers/ets_airspeed/ets_airspeed.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 1a7e068fe..d1a9fa57c 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -186,7 +186,6 @@ ETSAirspeed::collect() report.differential_pressure_filtered_pa = (float)diff_pres_pa; report.differential_pressure_raw_pa = (float)diff_pres_pa_raw; report.temperature = -1000.0f; - report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; if (_airspeed_pub > 0 && !(_pub_blocked)) { -- cgit v1.2.3 From 56e1fe382b77200b3826b1ba438fc49ebab8f8ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:10:15 +0200 Subject: Remove voltage field for MEAS sensor --- src/drivers/meas_airspeed/meas_airspeed.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index c0f3c28e0..7763f1057 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -265,7 +265,6 @@ MEASAirspeed::collect() } report.differential_pressure_raw_pa = diff_press_pa_raw; - report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; if (_airspeed_pub > 0 && !(_pub_blocked)) { -- cgit v1.2.3 From 3461d3d215843681b1537256d26b053e3f2b78e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:10:35 +0200 Subject: Introduce analog scaling parameter for analog airspeed sensor --- src/modules/sensors/sensor_params.c | 10 ++++++++-- src/modules/sensors/sensors.cpp | 18 +++++++++--------- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 5deb471d6..6b90a1239 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -194,16 +194,22 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); /** * Differential pressure sensor offset * + * The offset (zero-reading) in Pascal + * * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); /** - * Differential pressure sensor analog enabled + * Differential pressure sensor analog scaling + * + * Pick the appropriate scaling from the datasheet. + * this number defines the (linear) conversion from voltage + * to Pascal (pa). * * @group Sensor Calibration */ -PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); +PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); /** diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6e2d906e6..0cad0c0e5 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -248,7 +248,7 @@ private: float accel_offset[3]; float accel_scale[3]; float diff_pres_offset_pa; - float diff_pres_analog_enabled; + float diff_pres_analog_scale; int board_rotation; int external_mag_rotation; @@ -311,7 +311,7 @@ private: param_t mag_offset[3]; param_t mag_scale[3]; param_t diff_pres_offset_pa; - param_t diff_pres_analog_enabled; + param_t diff_pres_analog_scale; param_t rc_map_roll; param_t rc_map_pitch; @@ -590,7 +590,7 @@ Sensors::Sensors() : /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); - _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); + _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); @@ -798,7 +798,7 @@ Sensors::parameters_update() /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); - param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled)); + param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { @@ -1323,22 +1323,22 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ - float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) + float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor) /** * The voltage divider pulls the signal down, only act on * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) { + if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0)) { - float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor + float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale; _diff_pres.timestamp = t; - _diff_pres.differential_pressure_pa = diff_pres_pa; + _diff_pres.differential_pressure_pa = diff_pres_pa - _parameters.diff_pres_offset_pa; + _diff_pres.differential_pressure_raw_pa = diff_pres_pa; _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; _diff_pres.temperature = -1000.0f; - _diff_pres.voltage = voltage; /* announce the airspeed if needed, just publish else */ if (_diff_pres_pub > 0) { -- cgit v1.2.3 From 8d081a8b0dd002cda075ee9d8e087fdf54722ccf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:11:03 +0200 Subject: uORB: Remove voltage field from airspeed --- src/modules/uORB/topics/differential_pressure.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 01e14cda9..cd48d3cb2 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -58,7 +58,6 @@ struct differential_pressure_s { float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; -- cgit v1.2.3 From 25f3f6e7f2dd87a831d25a9348a67ed918407d96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:12:31 +0200 Subject: airspeed calibration improvements for analog sensors --- src/modules/commander/airspeed_calibration.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 923c1a3ff..12e527a68 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -76,7 +76,7 @@ int do_airspeed_calibration(int mavlink_fd) /* Reset sensor parameters */ struct airspeed_scale airscale = { - 0.0f, + diff_pres_offset, 1.0f, }; @@ -97,6 +97,12 @@ int do_airspeed_calibration(int mavlink_fd) if (!paramreset_successful) { warn("FAILED to reset - assuming analog"); mavlink_log_critical(mavlink_fd, "assuming analog sensor"); + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + close(diff_pres_sub); + return ERROR; + } } while (calibration_counter < calibration_count) { -- cgit v1.2.3 From 4c13c67504fb2b66c97ac6b902e67a864f1173e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:02:22 +0200 Subject: Hotfix: Close fd before reusing it again --- src/modules/commander/state_machine_helper.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6b96e3a3f..ca95f139e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -666,6 +666,8 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { + /* accel done, close it */ + close(fd); fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd < 0) { -- cgit v1.2.3 From 7ea76336ca03f724ba76661f986cc12cd0466ff8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:24:07 +0200 Subject: Better Doxygen for topics, no code changes --- src/drivers/drv_px4flow.h | 16 ++++++++++++++-- src/drivers/drv_range_finder.h | 9 +++++++++ src/drivers/drv_rc_input.h | 9 +++++++++ 3 files changed, 32 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index bef02d54e..76ec55c3e 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -46,10 +46,18 @@ #define PX4FLOW_DEVICE_PATH "/dev/px4flow" +/** + * @addtogroup topics + * @{ + */ + /** * Optical flow in NED body frame in SI units. * * @see http://en.wikipedia.org/wiki/International_System_of_Units + * + * @warning If possible the usage of the raw flow and performing rotation-compensation + * using the autopilot angular rate estimate is recommended. */ struct px4flow_report { @@ -57,14 +65,18 @@ struct px4flow_report { int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ - float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ - float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */ + float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */ + float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */ float ground_distance_m; /**< Altitude / distance to ground in meters */ uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ uint8_t sensor_id; /**< id of the sensor emitting the flow value */ }; +/** + * @} + */ + /* * ObjDev tag for px4flow data. */ diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index e45939b37..0f8362f58 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -50,6 +50,11 @@ enum RANGE_FINDER_TYPE { RANGE_FINDER_TYPE_LASER = 0, }; +/** + * @addtogroup topics + * @{ + */ + /** * range finder report structure. Reads from the device must be in multiples of this * structure. @@ -64,6 +69,10 @@ struct range_finder_report { uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ }; +/** + * @} + */ + /* * ObjDev tag for raw range finder data. */ diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 20763e265..47fa8fa59 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -67,6 +67,11 @@ */ #define RC_INPUT_RSSI_MAX 255 +/** + * @addtogroup topics + * @{ + */ + /** * Input signal type, value is a control position from zero to 100 * percent. @@ -141,6 +146,10 @@ struct rc_input_values { rc_input_t values[RC_INPUT_MAX_CHANNELS]; }; +/** + * @} + */ + /* * ObjDev tag for R/C inputs. */ -- cgit v1.2.3 From ee573fea9d27e7925b375abd95d540c0fda30d4c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:25:16 +0200 Subject: GPS driver: Print velocity as part of status command --- src/drivers/gps/gps.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 401b65dd4..34dd63086 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -127,7 +127,7 @@ private: /** * Try to configure the GPS, handle outgoing communication to the GPS */ - void config(); + void config(); /** * Trampoline to the worker task @@ -486,6 +486,8 @@ GPS::print_info() warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); + warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); -- cgit v1.2.3 From b97c867420f477f2c3a7dbc073975f5d872194cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:51:13 +0200 Subject: Add a check command and fix error reporting --- src/modules/commander/commander.cpp | 19 ++++++++++++------- src/modules/commander/state_machine_helper.cpp | 23 ++++++++++------------- src/modules/commander/state_machine_helper.h | 2 ++ 3 files changed, 24 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2058cd92e..106f05f57 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -286,15 +286,20 @@ int commander_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running"); - print_status(); + /* commands needing the app to run below */ + if (!thread_running) { + warnx("\tcommander not started"); + exit(1); + } - } else { - warnx("\tcommander not started"); - } + if (!strcmp(argv[1], "status")) { + print_status(); + exit(0); + } + if (!strcmp(argv[1], "check")) { + int checkres = prearm_check(&status, mavlink_fd); + warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); exit(0); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ca95f139e..d6b4d43ad 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -70,8 +70,6 @@ #endif static const int ERROR = -1; -static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); - // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even @@ -622,12 +620,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { int ret; + bool failed = false; int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); - ret = fd; + failed = true; goto system_eval; } @@ -635,6 +634,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret != OK) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + failed = true; goto system_eval; } @@ -646,22 +646,20 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) /* evaluate values */ float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - if (accel_scale < 9.78f || accel_scale > 9.83f) { + if (accel_scale < 9.75f || accel_scale > 9.85f) { mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); } if (accel_scale > 30.0f /* m/s^2 */) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; - } else { - ret = OK; } } else { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; } @@ -672,7 +670,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); - ret = fd; + failed = true; goto system_eval; } @@ -681,20 +679,19 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) ret = read(fd, &diff_pres, sizeof(diff_pres)); if (ret == sizeof(diff_pres)) { - if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { + if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) { mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); // XXX do not make this fatal yet - ret = OK; } } else { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; } } system_eval: close(fd); - return ret; + return (!failed); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 11072403e..ddc5bf154 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,4 +67,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); + #endif /* STATE_MACHINE_HELPER_H_ */ -- cgit v1.2.3 From a82d4fbb115e949a6a5c12d46308df1c20abfd99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:55:46 +0200 Subject: Hotfix: fix typo --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 106f05f57..6138a4c37 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -308,7 +308,7 @@ int commander_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "2")) { + if (!strcmp(argv[1], "disarm")) { arm_disarm(false, mavlink_fd, "command line"); exit(0); } -- cgit v1.2.3 From 79c5d434bd5911d11e6968e0a339b40a6f82e033 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 15:10:11 +0200 Subject: Commander: More hotfixes to prearm check routine --- src/modules/commander/commander.cpp | 5 ++++- src/modules/commander/state_machine_helper.cpp | 5 +++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6138a4c37..18761665a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -298,7 +298,9 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "check")) { - int checkres = prearm_check(&status, mavlink_fd); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + int checkres = prearm_check(&status, mavlink_fd_local); + close(mavlink_fd_local); warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); exit(0); } @@ -329,6 +331,7 @@ void usage(const char *reason) void print_status() { + warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); /* read all relevant states */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d6b4d43ad..5cbc95920 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -664,11 +664,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { + /* accel done, close it */ close(fd); fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) { + if (fd <= 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; @@ -693,5 +694,5 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) system_eval: close(fd); - return (!failed); + return (failed); } -- cgit v1.2.3 From a118e8dbdd72e23a47841577dfcd7a45007eaedc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 15:17:21 +0200 Subject: Make commander error message more verbose --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5cbc95920..ce1bd9ada 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -647,7 +647,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_scale < 9.75f || accel_scale > 9.85f) { - mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); + mavlink_log_info(mavlink_fd, "#audio: Accel calib. recommended (%8.4f).", (double)accel_scale); } if (accel_scale > 30.0f /* m/s^2 */) { -- cgit v1.2.3 From 11d9724563f005ed722d326e8bef125bfec29865 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 16:03:27 +0200 Subject: Stop warning users just because they shake a bit, be more strict to catch sensor failures or calibration errors in time --- src/modules/commander/state_machine_helper.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ce1bd9ada..363f5e8eb 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -644,14 +644,11 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret == sizeof(acc)) { /* evaluate values */ - float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - if (accel_scale < 9.75f || accel_scale > 9.85f) { - mavlink_log_info(mavlink_fd, "#audio: Accel calib. recommended (%8.4f).", (double)accel_scale); - } - - if (accel_scale > 30.0f /* m/s^2 */) { + if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); + mavlink_log_info(mavlink_fd, "#audio: hold still while arming"); /* this is frickin' fatal */ failed = true; goto system_eval; -- cgit v1.2.3 From aa055825984c121c0d2b74d81463282f6400688d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 21:04:09 +0200 Subject: FMUv1: Reduce excessive stack sizes --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index f2c7d22bf..6717ae19a 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -417,8 +417,8 @@ CONFIG_PREALLOC_TIMERS=50 # # Stack and heap information # -CONFIG_IDLETHREAD_STACKSIZE=4096 -CONFIG_USERMAIN_STACKSIZE=3500 +CONFIG_IDLETHREAD_STACKSIZE=3500 +CONFIG_USERMAIN_STACKSIZE=3000 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 -- cgit v1.2.3 From c474d2cbf1a9c78be5804b224c504b57f9248760 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 21:04:34 +0200 Subject: FMUv2: Reduce excessive stack sizes --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 7d5e6e9df..a55c95a29 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,8 +451,8 @@ CONFIG_PREALLOC_TIMERS=50 # # Stack and heap information # -CONFIG_IDLETHREAD_STACKSIZE=6000 -CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_IDLETHREAD_STACKSIZE=3500 +CONFIG_USERMAIN_STACKSIZE=3000 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 -- cgit v1.2.3 From a3210f31fe431b31b971f9d25d41a5c18734120c Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 11 Jul 2014 21:30:59 +0200 Subject: Create a mixer for the Wing Wing. --- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/mixers/FMU_wingwing | 70 ++++++++++++++++++++++++++++++++ 2 files changed, 71 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/mixers/FMU_wingwing diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index fff4b58df..75163168e 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -32,7 +32,7 @@ then param set FW_THR_CRUISE 0.65 fi -set MIXER FMU_Q +set MIXER FMU_wingwing # Provide ESC a constant 1000 us pulse set PWM_OUTPUTS 4 set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_wingwing b/ROMFS/px4fmu_common/mixers/FMU_wingwing new file mode 100644 index 000000000..806bfec73 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_wingwing @@ -0,0 +1,70 @@ +Delta-wing mixer for PX4FMU +=========================== + +Designed for Wing Wing Z-84 + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 6500 6500 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 -6500 -6500 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 -- cgit v1.2.3 From e4264aad825540c159888b68fc2a0dec599b2e9b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 11 Jul 2014 21:34:51 +0200 Subject: The wing wing is too small to have flaps, gimbals, etc. --- ROMFS/px4fmu_common/mixers/FMU_wingwing | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FMU_wingwing b/ROMFS/px4fmu_common/mixers/FMU_wingwing index 806bfec73..08333ba5c 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_wingwing +++ b/ROMFS/px4fmu_common/mixers/FMU_wingwing @@ -49,22 +49,3 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 - -Gimbal / flaps / payload mixer for last four channels ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 -- cgit v1.2.3 From 0a3a13ac7061566aac97321f09329b6228f0ed7e Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 11 Jul 2014 21:38:12 +0200 Subject: Updated following flight tests today. --- ROMFS/px4fmu_common/init.d/3033_wingwing | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 765685ccc..92f592aaf 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -30,14 +30,13 @@ then param set FW_RR_I 0.005 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 - param set FW_THR_CRUISE 0.65 param set MT_TKF_PIT_MAX 30.0 param set MT_ACC_D 0.2 param set MT_ACC_P 0.6 param set MT_A_LP 0.5 param set MT_PIT_OFF 0.1 param set MT_PIT_I 0.1 - param set MT_THR_OFF 0.12 + param set MT_THR_OFF 0.65 param set MT_THR_I 0.35 param set MT_THR_P 0.2 param set MT_THR_FF 1.5 -- cgit v1.2.3 From f8115e4e2e2cc0ad55949016c7872125a6727fac Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 11 Jul 2014 21:51:35 +0200 Subject: Renamed mixer file. --- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/mixers/FMU_wingwing | 51 -------------------------------- ROMFS/px4fmu_common/mixers/wingwing.mix | 51 ++++++++++++++++++++++++++++++++ 3 files changed, 52 insertions(+), 52 deletions(-) delete mode 100644 ROMFS/px4fmu_common/mixers/FMU_wingwing create mode 100644 ROMFS/px4fmu_common/mixers/wingwing.mix diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 75163168e..3cbbd555e 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -32,7 +32,7 @@ then param set FW_THR_CRUISE 0.65 fi -set MIXER FMU_wingwing +set MIXER wingwing # Provide ESC a constant 1000 us pulse set PWM_OUTPUTS 4 set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_wingwing b/ROMFS/px4fmu_common/mixers/FMU_wingwing deleted file mode 100644 index 08333ba5c..000000000 --- a/ROMFS/px4fmu_common/mixers/FMU_wingwing +++ /dev/null @@ -1,51 +0,0 @@ -Delta-wing mixer for PX4FMU -=========================== - -Designed for Wing Wing Z-84 - -This file defines mixers suitable for controlling a delta wing aircraft using -PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU -servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is -assumed to be unused. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -See the README for more information on the scaler format. - -Elevon mixers -------------- -Three scalers total (output, roll, pitch). - -On the assumption that the two elevon servos are physically reversed, the pitch -input is inverted between the two servos. - -The scaling factor for roll inputs is adjusted to implement differential travel -for the elevons. - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 -6000 -6000 0 -10000 10000 -S: 0 1 6500 6500 0 -10000 10000 - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 -6000 -6000 0 -10000 10000 -S: 0 1 -6500 -6500 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/wingwing.mix b/ROMFS/px4fmu_common/mixers/wingwing.mix new file mode 100644 index 000000000..08333ba5c --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/wingwing.mix @@ -0,0 +1,51 @@ +Delta-wing mixer for PX4FMU +=========================== + +Designed for Wing Wing Z-84 + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 6500 6500 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 -6500 -6500 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 -- cgit v1.2.3 From 899657613e44f0d4bbdb66644b35420d7381ff99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:09:25 +0200 Subject: airspeed cal: Improve user feedback --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 12e527a68..7599fc777 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -96,7 +96,7 @@ int do_airspeed_calibration(int mavlink_fd) if (!paramreset_successful) { warn("FAILED to reset - assuming analog"); - mavlink_log_critical(mavlink_fd, "assuming analog sensor"); + mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); -- cgit v1.2.3 From e3afb669cae54b042f453605601fed861c52ee45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:09:47 +0200 Subject: sensors: Fix usage of offset for measurements --- src/modules/sensors/sensor_params.c | 2 +- src/modules/sensors/sensors.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 6b90a1239..8f6e7abe6 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -205,7 +205,7 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); * * Pick the appropriate scaling from the datasheet. * this number defines the (linear) conversion from voltage - * to Pascal (pa). + * to Pascal (pa). For the MPXV7002DP this is 1000. * * @group Sensor Calibration */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 0cad0c0e5..cda79693d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1330,12 +1330,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw) * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0)) { + if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { - float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale; + float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; _diff_pres.timestamp = t; - _diff_pres.differential_pressure_pa = diff_pres_pa - _parameters.diff_pres_offset_pa; + _diff_pres.differential_pressure_pa = diff_pres_pa; _diff_pres.differential_pressure_raw_pa = diff_pres_pa; _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; _diff_pres.temperature = -1000.0f; -- cgit v1.2.3 From 9c63aba9a72083afe1e5f818c6559760c5b6b1cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:44:54 +0200 Subject: Ensure NaN check is executed before any parts of the filter are run. Fix GPS velocity reset for case of initialized GPS --- .../ekf_att_pos_estimator_main.cpp | 26 +++++++++++++--------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5d768b73d..bc5f709e5 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1140,6 +1140,20 @@ FixedwingEstimator::task_main() float initVelNED[3]; + // Guard before running any parts of the filter + // of NaN / invalid values + if (_ekf->statesInitialised) { + + // We're apparently initialized in this case now + + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } + } + /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1204,16 +1218,6 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } else if (_ekf->statesInitialised) { - // We're apparently initialized in this case now - - int check = check_filter_state(); - - if (check) { - // Let the system re-initialize itself - continue; - } - - // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 @@ -1281,7 +1285,7 @@ FixedwingEstimator::task_main() // run the fusion step _ekf->FuseVelposNED(); - } else if (_ekf->statesInitialised) { + } else if (!_gps_initialized && _ekf->statesInitialised) { // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; -- cgit v1.2.3 From 2fa4434a03abe057e65a5c8ba29f8cd9cd75ca93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 00:17:50 +0200 Subject: Limit output of driver to positive range --- src/modules/sensors/sensors.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cda79693d..1ffd7f02f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -501,6 +501,7 @@ Sensors::Sensors() : _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); + memset(&_diff_pres, 0, sizeof(_diff_pres)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1332,12 +1333,13 @@ Sensors::adc_poll(struct sensor_combined_s &raw) */ if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { - float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f; _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; - _diff_pres.differential_pressure_raw_pa = diff_pres_pa; - _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; + _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f); _diff_pres.temperature = -1000.0f; /* announce the airspeed if needed, just publish else */ -- cgit v1.2.3 From 065bf013a407da5e2a70db82013126c20e2ad429 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:29:30 +0200 Subject: More obvious error message for mission reject --- src/modules/navigator/mission.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 126877651..ba766cd10 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -277,7 +277,7 @@ Mission::check_dist_1wp() } else { /* item is too far from home */ - mavlink_log_info(_navigator->get_mavlink_fd(), "first wp is too far from home: %.1fm > %.1fm", (double)dist_to_1wp, (double)_param_dist_1wp.get()); + mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get()); return false; } } -- cgit v1.2.3 From 5616a07bf306b3fa2bc70078e9c8c26a086065dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:29:56 +0200 Subject: Final default values and more comments for params --- src/modules/navigator/mission_params.c | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 5cb3782c9..881caa24e 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -59,22 +59,26 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); /** - * Enable onboard mission + * Enable persistent onboard mission storage + * + * When enabled, missions that have been uploaded by the GCS are stored + * and reloaded after reboot persistently. * * @min 0 * @max 1 * @group Mission */ -PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0); +PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); /** * Maximal horizontal distance from home to first waypoint * - * Failsafe check to prevent running mission stored from previous flight on new place. - * Value < 0 means that check disabled. + * Failsafe check to prevent running mission stored from previous flight at a new takeoff location. + * Set a value of zero or less to disable. The mission will not be started if the current + * waypoint is more distant than MIS_DIS_1WP from the current position. * - * @min -1 - * @max 200 + * @min 0 + * @max 250 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 50); +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175); -- cgit v1.2.3 From 629ab5540fcb62b4c0ecc49e964f2d3fcc0b8a4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:30:49 +0200 Subject: Fix severity handling of text messages --- src/modules/mavlink/mavlink_main.cpp | 39 ++++++++++++++++++++++++------ src/modules/mavlink/mavlink_main.h | 46 ++++++++++++++++++++++++++++-------- 2 files changed, 68 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0d685c3d8..cd0581af4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -492,6 +492,7 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) // printf("logmsg: %s\n", txt); struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); + msg.severity = (unsigned char)cmd; Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { @@ -840,7 +841,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); - send_statustext("[mavlink pm] sending list"); + send_statustext_info("[pm] sending list"); } } break; @@ -864,7 +865,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown: %s", name); - send_statustext(buf); + send_statustext_info(buf); } else { /* set and send parameter */ @@ -901,17 +902,28 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav } int -Mavlink::send_statustext(const char *string) +Mavlink::send_statustext_info(const char *string) { - return send_statustext(MAV_SEVERITY_INFO, string); + return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); } int -Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) +Mavlink::send_statustext_critical(const char *string) +{ + return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); +} + +int +Mavlink::send_statustext_emergency(const char *string) +{ + return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); +} + +int +Mavlink::send_statustext(unsigned severity, const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; - statustext.severity = severity; int i = 0; @@ -929,6 +941,19 @@ Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) /* Enforce null termination */ statustext.text[i] = '\0'; + /* Map severity */ + switch (severity) { + case MAVLINK_IOC_SEND_TEXT_INFO: + statustext.severity = MAV_SEVERITY_INFO; + break; + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + statustext.severity = MAV_SEVERITY_CRITICAL; + break; + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + statustext.severity = MAV_SEVERITY_EMERGENCY; + break; + } + mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); return OK; @@ -1449,7 +1474,7 @@ Mavlink::task_main(int argc, char *argv[]) int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { - send_statustext(msg.text); + send_statustext(msg.severity, msg.text); } } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1f4fb759f..acfc8b90e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -157,9 +157,9 @@ public: */ int set_hil_enabled(bool hil_enabled); - void send_message(const mavlink_message_t *msg); + void send_message(const mavlink_message_t *msg); - void handle_message(const mavlink_message_t *msg); + void handle_message(const mavlink_message_t *msg); MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); @@ -174,17 +174,43 @@ public: mavlink_channel_t get_channel(); - void configure_stream_threadsafe(const char *stream_name, float rate); + void configure_stream_threadsafe(const char *stream_name, float rate); bool _task_should_exit; /**< if true, mavlink task should exit */ int get_mavlink_fd() { return _mavlink_fd; } - int send_statustext(const char *string); - int send_statustext(enum MAV_SEVERITY severity, const char *string); - MavlinkStream * get_streams() const { return _streams; } + /** + * Send a status text with loglevel INFO + * + * @param string the message to send (will be capped by mavlink max string length) + */ + int send_statustext_info(const char *string); + + /** + * Send a status text with loglevel CRITICAL + * + * @param string the message to send (will be capped by mavlink max string length) + */ + int send_statustext_critical(const char *string); + + /** + * Send a status text with loglevel EMERGENCY + * + * @param string the message to send (will be capped by mavlink max string length) + */ + int send_statustext_emergency(const char *string); + + /** + * Send a status text with loglevel + * + * @param string the message to send (will be capped by mavlink max string length) + * @param severity the log level, one of + */ + int send_statustext(unsigned severity, const char *string); + MavlinkStream * get_streams() const { return _streams; } - float get_rate_mult(); + float get_rate_mult(); /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } @@ -195,13 +221,13 @@ public: bool message_buffer_write(const void *ptr, int size); - void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } - void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } /** * Count a transmision error */ - void count_txerr(); + void count_txerr(); protected: Mavlink *next; -- cgit v1.2.3 From 7b768d11c3c8d57807495bf2abb324bb5f14aa14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:31:09 +0200 Subject: Improve mission feedback, fix compile errors --- src/modules/mavlink/mavlink_mission.cpp | 58 +++++++++++++++++++-------------- src/modules/mavlink/mavlink_mission.h | 36 ++++++++++---------- 2 files changed, 52 insertions(+), 42 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index ca01344d4..a62d34019 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -189,7 +189,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq) } else { if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); } - _mavlink->send_statustext("ERROR: wp index out of bounds"); + _mavlink->send_statustext_critical("ERROR: wp index out of bounds"); } } @@ -238,7 +238,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext("#audio: Unable to read from micro SD"); + _mavlink->send_statustext_critical("Unable to read from micro SD"); if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } } @@ -262,7 +262,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } } else { - _mavlink->send_statustext("ERROR: Waypoint index exceeds list capacity"); + _mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity"); if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); } } @@ -314,7 +314,7 @@ MavlinkMissionManager::eventloop() /* check for timed-out operations */ if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) { - _mavlink->send_statustext("Operation timeout"); + _mavlink->send_statustext_critical("Operation timeout"); if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); } @@ -390,6 +390,8 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); } } else { + _mavlink->send_statustext_critical("WPM: ERR: not all items sent -> IDLE"); + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); } } @@ -397,7 +399,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) } } else { - _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + _mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch"); if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } } @@ -421,18 +423,20 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } + + _mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID"); } } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); } - _mavlink->send_statustext("IGN WP CURR CMD: Not in list"); + _mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list"); } } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } - _mavlink->send_statustext("IGN WP CURR CMD: Busy"); + _mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy"); } } } @@ -459,6 +463,8 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } + + _mavlink->send_statustext_info("WPM: mission is empty"); } send_mission_count(msg->sysid, msg->compid, _count); @@ -466,7 +472,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } - _mavlink->send_statustext("IGN REQUEST LIST: Busy"); + _mavlink->send_statustext_critical("IGN REQUEST LIST: Busy"); } } } @@ -489,7 +495,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) _transfer_seq++; - } else if (wpr.seq == _transfer_seq - 1 && wpr.seq >= 0) { + } else if (wpr.seq == _transfer_seq - 1) { if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); } } else { @@ -506,36 +512,36 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) _state = MAVLINK_WPM_STATE_IDLE; send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected"); return; } /* double check bounds in case of items count changed */ - if (wpr.seq >= 0 && wpr.seq < _count) { + if (wpr.seq < _count) { send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [0, %u]", wpr.seq, msg->sysid, wpr.seq, _count - 1); } + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)_count - 1); } _state = MAVLINK_WPM_STATE_IDLE; send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected"); } } else if (_state == MAVLINK_WPM_STATE_IDLE) { if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); } - _mavlink->send_statustext("IGN MISSION_ITEM_REQUEST: No transfer"); + _mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST: No active transfer"); } else { if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); } - _mavlink->send_statustext("REJ. WP CMD: Busy"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy"); } } else { - _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch"); if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } } @@ -565,7 +571,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) /* alternate dataman ID anyway to let navigator know about changes */ update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); - _mavlink->send_statustext("WP COUNT 0: CLEAR MISSION"); + _mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION"); // TODO send ACK? return; @@ -588,19 +594,19 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) /* looks like our MISSION_REQUEST was lost, try again */ if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); } - _mavlink->send_statustext("WP CMD OK AGAIN"); + _mavlink->send_statustext_info("WP CMD OK TRY AGAIN"); } else { if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); } - _mavlink->send_statustext("REJ. WP CMD: Busy"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy"); return; } } else { if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); } - _mavlink->send_statustext("IGN MISSION_COUNT: Busy"); + _mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy"); return; } @@ -629,13 +635,13 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) } else if (_state == MAVLINK_WPM_STATE_IDLE) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); } - _mavlink->send_statustext("IGN MISSION_ITEM: No transfer"); + _mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer"); return; } else { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } - _mavlink->send_statustext("IGN MISSION_ITEM: Busy"); + _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); return; } @@ -645,6 +651,8 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) if (ret != OK) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); _state = MAVLINK_WPM_STATE_IDLE; return; @@ -656,7 +664,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext("#audio: Unable to write on micro SD"); + _mavlink->send_statustext_critical("Unable to write on micro SD"); _state = MAVLINK_WPM_STATE_IDLE; return; } @@ -674,6 +682,8 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) /* got all new mission items successfully */ if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } + _mavlink->send_statustext_info("WPM: Transfer complete."); + _state = MAVLINK_WPM_STATE_IDLE; if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { @@ -712,7 +722,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) } } else { - _mavlink->send_statustext("IGN WP CLEAR CMD: Busy"); + _mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy"); if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index d8695cf83..1964a45e8 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -142,35 +142,35 @@ public: void set_verbose(bool v) { _verbose = v; } private: - Mavlink* _mavlink; + Mavlink* _mavlink; mavlink_channel_t _channel; - uint8_t _comp_id; + uint8_t _comp_id; - enum MAVLINK_WPM_STATES _state; ///< Current state + enum MAVLINK_WPM_STATES _state; ///< Current state - uint64_t _time_last_recv; - uint64_t _time_last_sent; + uint64_t _time_last_recv; + uint64_t _time_last_sent; - uint32_t _action_timeout; - uint32_t _retry_timeout; - unsigned _max_count; ///< Maximal count of mission items + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximal count of mission items - int _dataman_id; ///< Dataman storage ID for active mission - unsigned _count; ///< Count of items in active mission - int _current_seq; ///< Current item sequence in active mission + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission int _transfer_dataman_id; ///< Dataman storage ID for current transmission - unsigned _transfer_count; ///< Items count in current transmission - int _transfer_seq; ///< Item sequence in current transmission - int _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) - uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission - uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + unsigned _transfer_seq; ///< Item sequence in current transmission + unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission + uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission int _offboard_mission_sub; int _mission_result_sub; - orb_advert_t _offboard_mission_pub; + orb_advert_t _offboard_mission_pub; - MavlinkRateLimiter _slow_rate_limiter; + MavlinkRateLimiter _slow_rate_limiter; bool _verbose; }; -- cgit v1.2.3 From a0d150332ab67dbc501b10fa8adb97d91a79f23c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 15:39:04 +0200 Subject: Add note to param about tubes --- src/modules/sensors/sensor_params.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 8f6e7abe6..38b190761 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -207,6 +207,9 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); * this number defines the (linear) conversion from voltage * to Pascal (pa). For the MPXV7002DP this is 1000. * + * NOTE: If the sensor always registers zero, try switching + * the static and dynamic tubes. + * * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); -- cgit v1.2.3 From 45617e9f4385e80846c571b237e220216192d6ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 16:09:14 +0200 Subject: Airspeed calibration improvement, enforce correct pitot order --- src/modules/commander/airspeed_calibration.cpp | 137 ++++++++++++++++++++----- src/modules/ekf_att_pos_estimator/InertialNav | 1 + 2 files changed, 112 insertions(+), 26 deletions(-) create mode 160000 src/modules/ekf_att_pos_estimator/InertialNav diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 7599fc777..a1a20abc9 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -64,14 +64,12 @@ int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); - const int calibration_count = 2000; + const unsigned calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; - int calibration_counter = 0; float diff_pres_offset = 0.0f; /* Reset sensor parameters */ @@ -105,29 +103,58 @@ int do_airspeed_calibration(int mavlink_fd) } } - while (calibration_counter < calibration_count) { + for (unsigned i = 0; i < (sizeof(diff_pres_offset) / sizeof(diff_pres_offset[0])); i++) { - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; + unsigned calibration_counter = 0; - int poll_ret = poll(fds, 1, 1000); + if (i == 0) { + mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + usleep(500 * 1000); + } - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; + float diff_pres_offset = 0.0f; - if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); - } + while (calibration_counter < calibration_count) { - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); - close(diff_pres_sub); - return ERROR; + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + + if (i > 0) { + + if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } + + /* do not log negative values in the second go */ + if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + close(diff_pres_sub); + return ERROR; + } + } + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } } } @@ -151,14 +178,72 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - tune_neutral(true); - close(diff_pres_sub); - return OK; - } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } + + /* wait 500 ms to ensure parameter propagated through the system */ + usleep(500 * 1000); + + calibration_counter = 0; + diff_pres_offset = 0.0f; + + /* just take a few samples and make sure pitot tubes are not reversed */ + while (calibration_counter < 50) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + float curr_avg = (diff_pres_offset / calibration_counter); + + if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } + + /* do not log negative values in the second go */ + if (curr_avg < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + close(diff_pres_sub); + + /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ + + diff_pres_offset = 0.0f; + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + close(diff_pres_sub); + return ERROR; + } + + /* save */ + (void)param_save_default(); + + return ERROR; + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } + } + + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + tune_neutral(true); + close(diff_pres_sub); + return OK; } diff --git a/src/modules/ekf_att_pos_estimator/InertialNav b/src/modules/ekf_att_pos_estimator/InertialNav new file mode 160000 index 000000000..8b65d755b --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/InertialNav @@ -0,0 +1 @@ +Subproject commit 8b65d755b8c4834f90a323172c25d91c45068e21 -- cgit v1.2.3 From 4dc65d6f09bcf891ee228ef9fc6b576251fc8b65 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 16:09:49 +0200 Subject: NuttX I2C fixes --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 2a94cc8e5..7e1b97bcf 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 2a94cc8e5babb7d5661aedc09201239d39644332 +Subproject commit 7e1b97bcf10d8495169eec355988ca5890bfd5df -- cgit v1.2.3 From 93d444d1aade59b5e88f41b71c842a00ab950c64 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:09:12 +1000 Subject: device: added a _device_id to all drivers this device ID identifies a specific device via the tuple of (bus, bus type, address, devtype). This allows device specific configuration data to be stored along with a device ID, so the code can know when the user has changed device configuration (such as removing an external compass), and either invalidate the device configuration or force the user to re-calibrate --- src/drivers/device/device.cpp | 12 ++++++++++++ src/drivers/device/device.h | 25 +++++++++++++++++++++++++ src/drivers/device/i2c.cpp | 8 +++++++- src/drivers/device/spi.cpp | 6 ++++++ src/drivers/drv_device.h | 7 +++++++ 5 files changed, 57 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index 683455149..f1f777dce 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -42,6 +42,7 @@ #include #include #include +#include namespace device { @@ -93,6 +94,13 @@ Device::Device(const char *name, _irq_attached(false) { sem_init(&_lock, 0, 1); + + /* setup a default device ID. When bus_type is UNKNOWN the + other fields are invalid */ + _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; + _device_id.devid_s.bus = 0; + _device_id.devid_s.address = 0; + _device_id.devid_s.devtype = 0; } Device::~Device() @@ -238,6 +246,10 @@ Device::write(unsigned offset, void *data, unsigned count) int Device::ioctl(unsigned operation, unsigned &arg) { + switch (operation) { + case DEVIOCGDEVICEID: + return (int)_device_id.devid; + } return -ENODEV; } diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index d99f22922..c98386eb0 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -124,9 +124,34 @@ public: */ virtual int ioctl(unsigned operation, unsigned &arg); + /* + device bus types for DEVID + */ + enum DeviceBusType { + DeviceBusType_UNKNOWN = 0, + DeviceBusType_I2C = 1, + DeviceBusType_SPI = 2 + }; + + /* + broken out + */ + struct DeviceStructure { + enum DeviceBusType bus_type; + uint8_t bus; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; + + union DeviceId { + struct DeviceStructure devid_s; + uint32_t devid; + }; + protected: const char *_name; /**< driver name */ bool _debug_enabled; /**< if true, debug messages are printed */ + union DeviceId _device_id; /**< device identifier information */ /** * Constructor diff --git a/src/drivers/device/i2c.cpp b/src/drivers/device/i2c.cpp index a416801eb..286778c01 100644 --- a/src/drivers/device/i2c.cpp +++ b/src/drivers/device/i2c.cpp @@ -62,6 +62,12 @@ I2C::I2C(const char *name, _frequency(frequency), _dev(nullptr) { + // fill in _device_id fields for a I2C device + _device_id.devid_s.bus_type = DeviceBusType_I2C; + _device_id.devid_s.bus = bus; + _device_id.devid_s.address = address; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; } I2C::~I2C() @@ -201,4 +207,4 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs) return ret; } -} // namespace device \ No newline at end of file +} // namespace device diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index 1ab1dc699..200acac05 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -75,6 +75,12 @@ SPI::SPI(const char *name, _dev(nullptr), _bus(bus) { + // fill in _device_id fields for a SPI device + _device_id.devid_s.bus_type = DeviceBusType_SPI; + _device_id.devid_s.bus = bus; + _device_id.devid_s.address = (uint8_t)device; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; } SPI::~SPI() diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index b310beb74..19d55cef3 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -59,4 +59,11 @@ /** check publication block status */ #define DEVIOCGPUBBLOCK _DEVICEIOC(1) +/** + * Return device ID, to enable matching of configuration parameters + * (such as compass offsets) to specific sensors + */ +#define DEVIOCGDEVICEID _DEVICEIOC(2) + + #endif /* _DRV_DEVICE_H */ -- cgit v1.2.3 From a2739707bb18a1aed0dcc0f809badce606aaed51 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:09:26 +1000 Subject: drv_mag: added devtypes for magnetometers --- src/drivers/drv_mag.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 06107bd3d..a259ac9c0 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -81,6 +81,13 @@ struct mag_scale { */ ORB_DECLARE(sensor_mag); + +/* + * mag device types, for _device_id + */ +#define DRV_MAG_DEVTYPE_HMC5883 1 +#define DRV_MAG_DEVTYPE_LSM303D 2 + /* * ioctl() definitions */ -- cgit v1.2.3 From 30a6a3d0b6cc53fef45264d140ce3026d986af83 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:09:38 +1000 Subject: hmc5883: setup device type --- src/drivers/hmc5883/hmc5883.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index c2e4700d8..cd2b5c48e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -341,6 +341,8 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : _bus(bus), _rotation(rotation) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + // enable debug() calls _debug_enabled = false; -- cgit v1.2.3 From c6b0dc1ee85348d8dbe398457e9631e095ec9c61 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:09:47 +1000 Subject: lsm303d: setup device type --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 7ba0fcc88..45e775055 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -530,6 +530,8 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _last_log_alarm_us(0), _rotation(rotation) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; + // enable debug() calls _debug_enabled = true; -- cgit v1.2.3 From 6cffa948fe6edc1e58b7d55acf119e8793461510 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:37:45 +1000 Subject: device: pass CDev::ioctl() to superclass this allows DEVIOCGDEVICEID to work. --- src/drivers/device/cdev.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 6e2ebb1f7..39fb89501 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -268,6 +268,13 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg) break; } + /* try the superclass. The different ioctl() function form + * means we need to copy arg */ + unsigned arg2 = arg; + int ret = Device::ioctl(cmd, arg2); + if (ret != -ENODEV) + return ret; + return -ENOTTY; } -- cgit v1.2.3 From 20cd5026d817a4f17b96906d1c93fc3cbaa498dd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 Jul 2014 09:49:43 +1000 Subject: device: use bitfields to keep DeviceStructure small this keeps it small enough to fit in a float, which makes it possible to see the full value in a MAVLink tlog Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/device/device.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index c98386eb0..7df234cab 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -134,11 +134,14 @@ public: }; /* - broken out + broken out device elements. The bitfields are used to keep + the overall value small enough to fit in a float accurately, + which makes it possible to transport over the MAVLink + parameter protocol without loss of information. */ struct DeviceStructure { - enum DeviceBusType bus_type; - uint8_t bus; // which instance of the bus type + enum DeviceBusType bus_type:3; + uint8_t bus:5; // which instance of the bus type uint8_t address; // address on the bus (eg. I2C address) uint8_t devtype; // device class specific device type }; -- cgit v1.2.3 From 8a3a87331da2077cd1da4c3da8fe2743f188a4a4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 Jul 2014 22:21:53 +1000 Subject: mpu6000: fixed internal/external mixup in pointers Thanks to Emile for spotting this! Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/mpu6000/mpu6000.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 5668b0865..1b3a96a0d 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1442,7 +1442,7 @@ void start(bool external_bus, enum Rotation rotation) { int fd; - MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; @@ -1595,7 +1595,7 @@ reset(bool external_bus) void info(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; if (*g_dev_ptr == nullptr) errx(1, "driver not running"); -- cgit v1.2.3 From b288b010f12213a4388b627bce7fd6cb4cdedea5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 18:42:00 +0200 Subject: tests, drive by: Fix double comparison, use reasonable margin based on context of test --- src/systemcmds/tests/test_bson.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c index 6130fe763..12d598df4 100644 --- a/src/systemcmds/tests/test_bson.c +++ b/src/systemcmds/tests/test_bson.c @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -123,7 +124,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE); return 1; } - if (node->d != sample_double) { + if (fabs(node->d - sample_double) > 1e-12) { warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double); return 1; } -- cgit v1.2.3 From ffebe45c4ce6cb248314141d91abcb74fbf9174e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 18:42:25 +0200 Subject: mavlink: Handle unhandled enum cases --- src/modules/mavlink/mavlink_messages.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c7ad605c5..7c864f127 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -170,6 +170,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_LAND: + /* fallthrough */ + case NAVIGATION_STATE_DESCEND: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -190,6 +192,17 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; + case NAVIGATION_STATE_OFFBOARD: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; + break; + + case NAVIGATION_STATE_MAX: + /* this is an unused case, ignore */ + break; + } *mavlink_custom_mode = custom_mode.data; -- cgit v1.2.3 From 0144a7dacfdb398c6133a9bf2df35facdbdfe6e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 18:43:09 +0200 Subject: mavlink: Optimize to native types where possible, move things to optimize alignment --- src/modules/mavlink/mavlink_mission.cpp | 6 +++--- src/modules/mavlink/mavlink_mission.h | 11 ++++++----- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index a62d34019..068774c47 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -61,8 +61,6 @@ static const int ERROR = -1; MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _mavlink(mavlink), - _channel(mavlink->get_channel()), - _comp_id(MAV_COMP_ID_MISSIONPLANNER), _state(MAVLINK_WPM_STATE_IDLE), _time_last_recv(0), _time_last_sent(0), @@ -82,7 +80,9 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _mission_result_sub(-1), _offboard_mission_pub(-1), _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), - _verbose(false) + _verbose(false), + _channel(mavlink->get_channel()), + _comp_id(MAV_COMP_ID_MISSIONPLANNER) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 1964a45e8..f63c32f24 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -143,8 +143,6 @@ public: private: Mavlink* _mavlink; - mavlink_channel_t _channel; - uint8_t _comp_id; enum MAVLINK_WPM_STATES _state; ///< Current state @@ -153,7 +151,7 @@ private: uint32_t _action_timeout; uint32_t _retry_timeout; - unsigned _max_count; ///< Maximal count of mission items + unsigned _max_count; ///< Maximum number of mission items int _dataman_id; ///< Dataman storage ID for active mission unsigned _count; ///< Count of items in active mission @@ -163,8 +161,8 @@ private: unsigned _transfer_count; ///< Items count in current transmission unsigned _transfer_seq; ///< Item sequence in current transmission unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) - uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission - uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission + unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission + unsigned _transfer_partner_compid; ///< Partner component ID for current transmission int _offboard_mission_sub; int _mission_result_sub; @@ -173,4 +171,7 @@ private: MavlinkRateLimiter _slow_rate_limiter; bool _verbose; + + mavlink_channel_t _channel; + uint8_t _comp_id; }; -- cgit v1.2.3 From 005dd206d17920761f8e27b21d54770da59faa13 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 12 Jul 2014 09:51:22 +1000 Subject: hmc5883: periodically check the config and range registers this copes with I2C comms errors causing the range or config registers to become corrupted, leading to bad reading. This is easily reproducible with a 1.3m I2C cable in the same run of cable as a GPS UART cable. The error happens every half hour or so. Conflicts: mavlink/include/mavlink/v1.0 src/drivers/hmc5883/hmc5883.cpp --- src/drivers/hmc5883/hmc5883.cpp | 141 ++++++++++++++++++++++++++++++++-------- 1 file changed, 114 insertions(+), 27 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index cd2b5c48e..4aef43102 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -166,6 +166,8 @@ private: perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + perf_counter_t _range_errors; + perf_counter_t _conf_errors; /* status reporting */ bool _sensor_ok; /**< sensor was found and reports ok */ @@ -176,6 +178,9 @@ private: struct mag_report _last_report; /**< used for info() */ + uint8_t _range_bits; + uint8_t _conf_reg; + /** * Test whether the device supported by the driver is present at a * specific address. @@ -233,6 +238,23 @@ private: */ int set_range(unsigned range); + /** + * check the sensor range. + * + * checks that the range of the sensor is correctly set, to + * cope with communication errors causing the range to change + */ + void check_range(void); + + /** + * check the sensor configuration. + * + * checks that the config of the sensor is correctly set, to + * cope with communication errors causing the configuration to + * change + */ + void check_conf(void); + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -336,10 +358,15 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), + _range_errors(perf_alloc(PC_COUNT, "hmc5883_range_errors")), + _conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")), _sensor_ok(false), _calibrated(false), _bus(bus), - _rotation(rotation) + _rotation(rotation), + _last_report{0}, + _range_bits(0), + _conf_reg(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; @@ -373,6 +400,8 @@ HMC5883::~HMC5883() perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_buffer_overflows); + perf_free(_range_errors); + perf_free(_conf_errors); } int @@ -403,45 +432,43 @@ out: int HMC5883::set_range(unsigned range) { - uint8_t range_bits; - if (range < 1) { - range_bits = 0x00; + _range_bits = 0x00; _range_scale = 1.0f / 1370.0f; _range_ga = 0.88f; } else if (range <= 1) { - range_bits = 0x01; + _range_bits = 0x01; _range_scale = 1.0f / 1090.0f; _range_ga = 1.3f; } else if (range <= 2) { - range_bits = 0x02; + _range_bits = 0x02; _range_scale = 1.0f / 820.0f; _range_ga = 1.9f; } else if (range <= 3) { - range_bits = 0x03; + _range_bits = 0x03; _range_scale = 1.0f / 660.0f; _range_ga = 2.5f; } else if (range <= 4) { - range_bits = 0x04; + _range_bits = 0x04; _range_scale = 1.0f / 440.0f; _range_ga = 4.0f; } else if (range <= 4.7f) { - range_bits = 0x05; + _range_bits = 0x05; _range_scale = 1.0f / 390.0f; _range_ga = 4.7f; } else if (range <= 5.6f) { - range_bits = 0x06; + _range_bits = 0x06; _range_scale = 1.0f / 330.0f; _range_ga = 5.6f; } else { - range_bits = 0x07; + _range_bits = 0x07; _range_scale = 1.0f / 230.0f; _range_ga = 8.1f; } @@ -451,7 +478,7 @@ int HMC5883::set_range(unsigned range) /* * Send the command to set the range */ - ret = write_reg(ADDR_CONF_B, (range_bits << 5)); + ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); if (OK != ret) perf_count(_comms_errors); @@ -462,7 +489,53 @@ int HMC5883::set_range(unsigned range) if (OK != ret) perf_count(_comms_errors); - return !(range_bits_in == (range_bits << 5)); + return !(range_bits_in == (_range_bits << 5)); +} + +/** + check that the range register has the right value. This is done + periodically to cope with I2C bus noise causing the range of the + compass changing. + */ +void HMC5883::check_range(void) +{ + int ret; + + uint8_t range_bits_in; + ret = read_reg(ADDR_CONF_B, range_bits_in); + if (OK != ret) { + perf_count(_comms_errors); + return; + } + if (range_bits_in != (_range_bits<<5)) { + perf_count(_range_errors); + ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); + if (OK != ret) + perf_count(_comms_errors); + } +} + +/** + check that the configuration register has the right value. This is + done periodically to cope with I2C bus noise causing the + configuration of the compass to change. + */ +void HMC5883::check_conf(void) +{ + int ret; + + uint8_t conf_reg_in; + ret = read_reg(ADDR_CONF_A, conf_reg_in); + if (OK != ret) { + perf_count(_comms_errors); + return; + } + if (conf_reg_in != _conf_reg) { + perf_count(_conf_errors); + ret = write_reg(ADDR_CONF_A, _conf_reg); + if (OK != ret) + perf_count(_comms_errors); + } } int @@ -796,7 +869,7 @@ HMC5883::collect() } report; int ret = -EIO; uint8_t cmd; - + uint8_t check_counter; perf_begin(_sample_perf); struct mag_report new_report; @@ -895,6 +968,21 @@ HMC5883::collect() /* notify anyone waiting for data */ poll_notify(POLLIN); + /* + periodically check the range register and configuration + registers. With a bad I2C cable it is possible for the + registers to become corrupt, leading to bad readings. It + doesn't happen often, but given the poor cables some + vehicles have it is worth checking for. + */ + check_counter = perf_event_count(_sample_perf) % 256; + if (check_counter == 0) { + check_range(); + } + if (check_counter == 128) { + check_conf(); + } + ret = OK; out: @@ -1168,25 +1256,24 @@ int HMC5883::set_excitement(unsigned enable) { int ret; /* arm the excitement strap */ - uint8_t conf_reg; - ret = read_reg(ADDR_CONF_A, conf_reg); + ret = read_reg(ADDR_CONF_A, _conf_reg); if (OK != ret) perf_count(_comms_errors); if (((int)enable) < 0) { - conf_reg |= 0x01; + _conf_reg |= 0x01; } else if (enable > 0) { - conf_reg |= 0x02; + _conf_reg |= 0x02; } else { - conf_reg &= ~0x03; + _conf_reg &= ~0x03; } - // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg); + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); - ret = write_reg(ADDR_CONF_A, conf_reg); + ret = write_reg(ADDR_CONF_A, _conf_reg); if (OK != ret) perf_count(_comms_errors); @@ -1196,7 +1283,7 @@ int HMC5883::set_excitement(unsigned enable) //print_info(); - return !(conf_reg == conf_reg_ret); + return !(_conf_reg == conf_reg_ret); } int @@ -1257,12 +1344,12 @@ const int ERROR = -1; HMC5883 *g_dev_int; HMC5883 *g_dev_ext; -void hmc5883_usage(); void start(int bus, enum Rotation rotation); void test(int bus); void reset(int bus); void info(int bus); int calibrate(int bus); +void usage(); /** * Start the driver. @@ -1511,10 +1598,8 @@ info(int bus) exit(0); } -} // namespace - void -hmc5883_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'"); warnx("options:"); @@ -1526,6 +1611,8 @@ hmc5883_usage() #endif } +} // namespace + int hmc5883_main(int argc, char *argv[]) { @@ -1551,7 +1638,7 @@ hmc5883_main(int argc, char *argv[]) calibrate = true; break; default: - hmc5883_usage(); + hmc5883::usage(); exit(0); } } -- cgit v1.2.3 From 67e3a904b6526f9268530d9c8e9529585c0be235 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:05:13 +0200 Subject: fix ms5611 code style for usage call --- src/drivers/ms5611/ms5611.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 7a8d2eecf..fe669b5f5 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -781,6 +781,7 @@ void test(); void reset(); void info(); void calibrate(unsigned altitude); +void usage(); /** * MS5611 crc4 cribbed from the datasheet @@ -1057,16 +1058,16 @@ calibrate(unsigned altitude) exit(0); } -} // namespace - void -ms5611_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); warnx("options:"); warnx(" -X (external bus)"); } +} // namespace + int ms5611_main(int argc, char *argv[]) { @@ -1080,7 +1081,7 @@ ms5611_main(int argc, char *argv[]) external_bus = true; break; default: - ms5611_usage(); + ms5611::usage(); exit(0); } } -- cgit v1.2.3 From 0d1ac4235411e8f05f96bcbe51558d92f0d86cf6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:10:08 +0200 Subject: airspeed calibration: Update and resolve compile errors --- src/modules/commander/airspeed_calibration.cpp | 75 ++++++++++++-------------- 1 file changed, 33 insertions(+), 42 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a1a20abc9..421e53ae0 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -51,6 +51,7 @@ #include #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -103,58 +104,48 @@ int do_airspeed_calibration(int mavlink_fd) } } - for (unsigned i = 0; i < (sizeof(diff_pres_offset) / sizeof(diff_pres_offset[0])); i++) { + unsigned calibration_counter = 0; - unsigned calibration_counter = 0; - - if (i == 0) { - mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); - usleep(500 * 1000); - } - - float diff_pres_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + usleep(500 * 1000); - if (i > 0) { + while (calibration_counter < calibration_count) { - if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); - usleep(5000 * 1000); - continue; - } + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; - /* do not log negative values in the second go */ - if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); - close(diff_pres_sub); - return ERROR; - } - } + int poll_ret = poll(fds, 1, 1000); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); - } + if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + /* do not log negative values in the second go */ + if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); return ERROR; } + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; } } -- cgit v1.2.3 From e6b5e3ae613e89189ac69cf0b174b10002d51068 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:22:36 +0200 Subject: Add note about need to scan external buses first --- ROMFS/px4fmu_common/init.d/rc.sensors | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 1e14930fe..be54ea98b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -24,6 +24,7 @@ fi if ver hwcmp PX4FMU_V2 then + # IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST if lsm303d start then echo "[init] Using LSM303D" -- cgit v1.2.3 From 8960c9e0a8b165ce15cf1864d61f8f764935a081 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:28:10 +0200 Subject: better submodule instructions --- Tools/check_submodules.sh | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index cc6e7d1c0..8adc6b6c7 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -12,8 +12,12 @@ if [ -d NuttX/nuttx ]; if [ -z "$STATUSRETVAL" ]; then echo "Checked NuttX submodule, correct version found" else - echo "NuttX sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + echo "" + echo "" + echo " NuttX sub repo not at correct version. Try 'git submodule update'" + echo " or follow instructions on http://pixhawk.org/dev/git/submodules" + echo "" + echo " DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!" echo "" echo "" echo "New commits required:" @@ -33,6 +37,8 @@ if [ -d mavlink/include/mavlink/v1.0 ]; if [ -z "$STATUSRETVAL" ]; then echo "Checked mavlink submodule, correct version found" else + echo "" + echo "" echo "mavlink sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" echo "" -- cgit v1.2.3 From d6632ee2dda39de78be1bbfa6754af8b59c58655 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:34:06 +0200 Subject: ardrone: Optimize for size, since performance is good at any rate --- src/drivers/ardrone_interface/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/ardrone_interface/module.mk b/src/drivers/ardrone_interface/module.mk index d8e6c76c6..285db1351 100644 --- a/src/drivers/ardrone_interface/module.mk +++ b/src/drivers/ardrone_interface/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = ardrone_interface SRCS = ardrone_interface.c \ ardrone_motor_control.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 85301e1172a0ab7cf726ba8ffc5386ab7ede363d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:34:28 +0200 Subject: frsky: Optimize for size --- src/drivers/frsky_telemetry/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk index 9a49589ee..78ad6f67e 100644 --- a/src/drivers/frsky_telemetry/module.mk +++ b/src/drivers/frsky_telemetry/module.mk @@ -41,3 +41,5 @@ SRCS = frsky_data.c \ frsky_telemetry.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 144bb89e027701f0b4c1661685d770a013c100f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:34:40 +0200 Subject: HoTT: optimize for size --- src/drivers/hott/hott_sensors/module.mk | 2 ++ src/drivers/hott/hott_telemetry/module.mk | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk index b5f5762ba..47aea6caf 100644 --- a/src/drivers/hott/hott_sensors/module.mk +++ b/src/drivers/hott/hott_sensors/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = hott_sensors SRCS = hott_sensors.cpp \ ../messages.cpp \ ../comms.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk index b19cbd14c..cd7bdbc85 100644 --- a/src/drivers/hott/hott_telemetry/module.mk +++ b/src/drivers/hott/hott_telemetry/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = hott_telemetry SRCS = hott_telemetry.cpp \ ../messages.cpp \ ../comms.cpp + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From aaf2652e26106b0850226f25cd12ce1304775522 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:34:58 +0200 Subject: MKBLCTRL: optimize for size --- src/drivers/mkblctrl/module.mk | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/mkblctrl/module.mk b/src/drivers/mkblctrl/module.mk index 3ac263b00..6daa14aa5 100644 --- a/src/drivers/mkblctrl/module.mk +++ b/src/drivers/mkblctrl/module.mk @@ -37,6 +37,8 @@ MODULE_COMMAND = mkblctrl -SRCS = mkblctrl.cpp +SRCS = mkblctrl.cpp INCLUDE_DIRS += $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 70d0ff492220371c57f9ee3d1f4fedb2fcf1199f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:35:11 +0200 Subject: SF0X: optimize for size --- src/drivers/sf0x/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/sf0x/module.mk b/src/drivers/sf0x/module.mk index dc93a90e7..dc2c66d56 100644 --- a/src/drivers/sf0x/module.mk +++ b/src/drivers/sf0x/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = sf0x SRCS = sf0x.cpp + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 959bf6a2c8ac656b1536762a2a7aea0c5349f5d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:35:49 +0200 Subject: ll40ls: Optimize for size --- src/drivers/ll40ls/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/ll40ls/module.mk b/src/drivers/ll40ls/module.mk index ab7d43269..fb627afee 100644 --- a/src/drivers/ll40ls/module.mk +++ b/src/drivers/ll40ls/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = ll40ls SRCS = ll40ls.cpp + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 8a6e69ed671d3de6b267518f1cd425b24e48c71e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 20:08:36 +0200 Subject: Fix up RAM usage of fixed wing apps --- src/modules/fw_att_control/module.mk | 2 ++ src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/fw_pos_control_l1/module.mk | 4 +++- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk index 1e600757e..89c6494c5 100644 --- a/src/modules/fw_att_control/module.mk +++ b/src/modules/fw_att_control/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = fw_att_control SRCS = fw_att_control_main.cpp \ fw_att_control_params.c + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3d6c62434..98ccd09a5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1445,7 +1445,7 @@ FixedwingPositionControl::start() _control_task = task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 3500, + 2000, (main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index af155fe08..15b575b50 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -43,3 +43,5 @@ SRCS = fw_pos_control_l1_main.cpp \ mtecs/mTecs.cpp \ mtecs/limitoverride.cpp \ mtecs/mTecs_params.c + +MODULE_STACKSIZE = 1200 -- cgit v1.2.3 From f32a51f51597bb66c7e404a7dc8e723c32f44743 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 20:14:01 +0200 Subject: Fixed battery ignore voltage to a higher value --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6e2d906e6..c40e52a0d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -133,7 +133,7 @@ #endif #define BATT_V_LOWPASS 0.001f -#define BATT_V_IGNORE_THRESHOLD 3.5f +#define BATT_V_IGNORE_THRESHOLD 4.8f /** * HACK - true temperature is much less than indicated temperature in baro, -- cgit v1.2.3 From c059fb03ea5ba85446d93df4db73e867f01e288d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 21:33:09 +0200 Subject: blinkm: Make driver flash efficient --- src/drivers/blinkm/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/blinkm/module.mk b/src/drivers/blinkm/module.mk index b48b90f3f..c90673317 100644 --- a/src/drivers/blinkm/module.mk +++ b/src/drivers/blinkm/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = blinkm SRCS = blinkm.cpp + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 69937702b8ff4ee052e960f6427e4653b4743e2a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 21:33:21 +0200 Subject: gps: Flash efficiency --- src/drivers/gps/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index eb382c4b2..b00818424 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -43,3 +43,5 @@ SRCS = gps.cpp \ ubx.cpp MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 5f8baed876a805c2cfae7c0ed0250ae20b501336 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 21:33:34 +0200 Subject: mb12xx: flash efficiency --- src/drivers/mb12xx/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/mb12xx/module.mk b/src/drivers/mb12xx/module.mk index 4e00ada02..d751e93e4 100644 --- a/src/drivers/mb12xx/module.mk +++ b/src/drivers/mb12xx/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = mb12xx SRCS = mb12xx.cpp + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 7bf0e6f3e26ee9e10073293677563f6862758557 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 21:47:26 +0200 Subject: Better airspeed feedback --- src/modules/commander/airspeed_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 421e53ae0..a1b8816ff 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -200,13 +200,13 @@ int do_airspeed_calibration(int mavlink_fd) float curr_avg = (diff_pres_offset / calibration_counter); if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", curr_avg); usleep(5000 * 1000); continue; } /* do not log negative values in the second go */ - if (curr_avg < calc_indicated_airspeed(-5.0f)) { + if (curr_avg < -calc_indicated_airspeed(5.0f)) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); -- cgit v1.2.3 From 67d23253c3b3ec04bfd9d5f7e09d3d405ceba7c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:00:50 +0200 Subject: airspeed cal build fix --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a1b8816ff..529a5b32f 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -200,7 +200,7 @@ int do_airspeed_calibration(int mavlink_fd) float curr_avg = (diff_pres_offset / calibration_counter); if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", curr_avg); + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); usleep(5000 * 1000); continue; } -- cgit v1.2.3 From fe5d35bc546232b2d897e7509c28441178b6e7ea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:04:04 +0200 Subject: Reduce IO buf space reasonably --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 6717ae19a..a651faffa 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -679,7 +679,7 @@ CONFIG_BUILTIN=y # # Standard C Library Options # -CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STDIO_BUFFER_SIZE=180 CONFIG_STDIO_LINEBUFFER=y CONFIG_NUNGET_CHARS=2 CONFIG_LIB_HOMEDIR="/" -- cgit v1.2.3 From 4824a4e8ac93d992e1401f768291cca65ba46acc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:18:59 +0200 Subject: Fix for dynamic / static part of calibration --- src/modules/commander/airspeed_calibration.cpp | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 529a5b32f..01db34be2 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -121,19 +121,6 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); - usleep(5000 * 1000); - continue; - } - - /* do not log negative values in the second go */ - if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); - close(diff_pres_sub); - return ERROR; - } - diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; @@ -197,16 +184,16 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; - float curr_avg = (diff_pres_offset / calibration_counter); + float curr_avg_airspeed = calc_indicated_airspeed(diff_pres_offset / calibration_counter); - if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { + if (fabsf(curr_avg) < 10.0f) { mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); usleep(5000 * 1000); continue; } /* do not log negative values in the second go */ - if (curr_avg < -calc_indicated_airspeed(5.0f)) { + if (curr_avg_airspeed < -calc_indicated_airspeed(5.0f)) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); -- cgit v1.2.3 From 34abf5c40cda538f204d313cc49715b5a938d168 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:50:56 +0200 Subject: airspeed cal: Fix up logic --- src/modules/commander/airspeed_calibration.cpp | 25 +++++++++++-------------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 01db34be2..47c9c7af5 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -165,11 +165,8 @@ int do_airspeed_calibration(int mavlink_fd) /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - calibration_counter = 0; - diff_pres_offset = 0.0f; - /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 50) { + while (calibration_counter < 10) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -181,19 +178,15 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; - - float curr_avg_airspeed = calc_indicated_airspeed(diff_pres_offset / calibration_counter); - - if (fabsf(curr_avg) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); - usleep(5000 * 1000); + if (fabsf(diff_pres.differential_pressure_raw_pa) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f Pa)", + (double)diff_pres.differential_pressure_raw_pa); + usleep(3000 * 1000); continue; } - /* do not log negative values in the second go */ - if (curr_avg_airspeed < -calc_indicated_airspeed(5.0f)) { + /* do not allow negative values */ + if (diff_pres.differential_pressure_raw_pa < 0.0f) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); @@ -209,7 +202,11 @@ int do_airspeed_calibration(int mavlink_fd) /* save */ (void)param_save_default(); + close(diff_pres_sub); return ERROR; + } else { + close(diff_pres_sub); + return OK; } } else if (poll_ret == 0) { -- cgit v1.2.3 From 65409ad2c8ba10d95cbcfa088951abc40eb46774 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 23:30:00 +0200 Subject: Airspeed calibration improvements --- src/modules/commander/airspeed_calibration.cpp | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 47c9c7af5..d52cafb8f 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -125,7 +125,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { @@ -165,6 +165,8 @@ int do_airspeed_calibration(int mavlink_fd) /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); + calibration_counter = 0; + /* just take a few samples and make sure pitot tubes are not reversed */ while (calibration_counter < 10) { @@ -178,16 +180,18 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (fabsf(diff_pres.differential_pressure_raw_pa) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f Pa)", - (double)diff_pres.differential_pressure_raw_pa); + float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; + + if (fabsf(calibrated_pa) < 9.0f) { + mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%.1f Pa, #h101)", + (double)calibrated_pa); usleep(3000 * 1000); continue; } /* do not allow negative values */ - if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + if (calibrated_pa < 0.0f) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static vs dynamic ports,restart"); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -205,8 +209,9 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); return ERROR; } else { - close(diff_pres_sub); - return OK; + mavlink_log_info(mavlink_fd, "positive pressure: (%.1f Pa)", + (double)diff_pres.differential_pressure_raw_pa); + break; } } else if (poll_ret == 0) { @@ -217,6 +222,8 @@ int do_airspeed_calibration(int mavlink_fd) } } + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); tune_neutral(true); close(diff_pres_sub); -- cgit v1.2.3 From cdfbe9bcc41a6e8e8b2a6f95bd69283ee5176966 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 23:30:34 +0200 Subject: px4io: Do not forward excessively low battery voltages --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 24da4c68b..7d78b0d27 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1383,7 +1383,7 @@ void PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) { /* only publish if battery has a valid minimum voltage */ - if (vbatt <= 3300) { + if (vbatt <= 4900) { return; } -- cgit v1.2.3 From 1b6ad3e1990bec84489f949b195adba4412e1c7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:37:10 +0200 Subject: ekf: Logic cleanup and print code cleanup --- .../ekf_att_pos_estimator_main.cpp | 137 ++++++++------------- 1 file changed, 49 insertions(+), 88 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index bc5f709e5..5ce6bdb0a 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -559,61 +559,26 @@ FixedwingEstimator::check_filter_state() int check = _ekf->CheckAndBound(&ekf_report); - const char* ekfname = "att pos estimator: "; - - switch (check) { - case 0: - /* all ok */ - break; - case 1: - { - const char* str = "NaN in states, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 2: - { - const char* str = "stale IMU data, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 3: - { - const char* str = "switching to dynamic state"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 4: - { - const char* str = "excessive gyro offsets"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 5: - { - const char* str = "GPS velocity divergence"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 6: - { - const char* str = "excessive covariances"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; + const char* const feedback[] = { 0, + "NaN in states, resetting", + "stale IMU data, resetting", + "got initial position lock", + "excessive gyro offsets", + "GPS velocity divergence", + "excessive covariances", + "unknown condition"}; + + // Print out error condition + if (check) { + unsigned warn_index = static_cast(check); + unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0])); + + if (max_warn_index < warn_index) { + warn_index = max_warn_index; } - default: - { - const char* str = "unknown reset condition"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - } + warnx("reset: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]); } struct estimator_status_report rep; @@ -1140,20 +1105,6 @@ FixedwingEstimator::task_main() float initVelNED[3]; - // Guard before running any parts of the filter - // of NaN / invalid values - if (_ekf->statesInitialised) { - - // We're apparently initialized in this case now - - int check = check_filter_state(); - - if (check) { - // Let the system re-initialize itself - continue; - } - } - /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1216,8 +1167,17 @@ FixedwingEstimator::task_main() _baro_gps_offset = 0.0f; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + } else if (_ekf->statesInitialised) { + // We're apparently initialized in this case now + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } + // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 @@ -1285,7 +1245,7 @@ FixedwingEstimator::task_main() // run the fusion step _ekf->FuseVelposNED(); - } else if (!_gps_initialized && _ekf->statesInitialised) { + } else if (!_gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; @@ -1307,7 +1267,7 @@ FixedwingEstimator::task_main() _ekf->fusePosData = false; } - if (newHgtData && _ekf->statesInitialised) { + if (newHgtData) { // Could use a blend of GPS and baro alt data if desired _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; @@ -1321,7 +1281,7 @@ FixedwingEstimator::task_main() } // Fuse Magnetometer Measurements - if (newDataMag && _ekf->statesInitialised) { + if (newDataMag) { _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data @@ -1335,7 +1295,7 @@ FixedwingEstimator::task_main() } // Fuse Airspeed Measurements - if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { + if (newAdsData && _ekf->VtasMeas > 7.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); @@ -1403,7 +1363,7 @@ FixedwingEstimator::task_main() _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); - _airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s; + _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; /* crude land detector for fixedwing only, @@ -1494,27 +1454,28 @@ FixedwingEstimator::task_main() } - } - - } + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = _ekf->P[14][14]; - _wind.covariance_east = _ekf->P[15][15]; + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } + } - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); } + } + } } -- cgit v1.2.3 From be5ac5e4127d09187ef0e19a7955cca9b0f1b378 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:38:52 +0200 Subject: Fix mavlink log header docs. --- src/include/mavlink/mavlink_log.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 0ea655cac..6d56c546a 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions -- cgit v1.2.3 From 716e20fab6d63f3dc6f80c70f382d6b0f45f9340 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:39:13 +0200 Subject: Fix accel cal docs. --- src/modules/commander/accelerometer_calibration.cpp | 3 +-- src/modules/commander/accelerometer_calibration.h | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index be465ab76..7a4e7a766 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 1cf9c0977..6b7e16d44 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions -- cgit v1.2.3 From 259014b0d50426b0fbed3b9eac5a1d34aaa5b211 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:40:09 +0200 Subject: Documentation only and unused defines only cleanup. --- src/modules/commander/airspeed_calibration.h | 2 +- src/modules/commander/baro_calibration.cpp | 2 +- src/modules/commander/baro_calibration.h | 2 +- src/modules/commander/calibration_messages.h | 3 +-- src/modules/commander/calibration_routines.cpp | 3 +-- src/modules/commander/calibration_routines.h | 3 +-- src/modules/commander/mag_calibration.h | 2 +- src/modules/commander/state_machine_helper.h | 10 ++++------ 8 files changed, 11 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h index 71c701fc2..8c6b65ff1 100644 --- a/src/modules/commander/airspeed_calibration.h +++ b/src/modules/commander/airspeed_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 3123c4087..da98644b4 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h index bc42bc6cf..ce78ae700 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/commander/baro_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index fd8b8564d..b1e209efc 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 43f341ae7..5796204bf 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index e3e7fbafd..3c8e49ee9 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h index a101cd7b1..c14f948cc 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index ddc5bf154..bb1b87e71 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,14 +34,14 @@ /** * @file state_machine_helper.h * State machine helper functions definitions + * + * @author Thomas Gubler + * @author Julian Oes */ #ifndef STATE_MACHINE_HELPER_H_ #define STATE_MACHINE_HELPER_H_ -#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor) -#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock - #include #include #include -- cgit v1.2.3 From ea11bc6c4bf7180ae4e321eebcaf817eebb18e6c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:41:39 +0200 Subject: Command handling: Fix up local variable usage and status prints. --- src/modules/commander/commander.cpp | 67 +++++++++++++++++++++---------------- 1 file changed, 38 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e14dff197..009e0ac01 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -403,10 +403,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char return arming_res; } -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) +bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, + struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, + struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } @@ -425,7 +427,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); + transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); @@ -434,43 +436,43 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_ret = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_ret = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_ret = main_state_transition(status, MAIN_STATE_ACRO); + main_ret = main_state_transition(status_local, MAIN_STATE_ACRO); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ - main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD); + main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD); } } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_ret = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_ret = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); } } } @@ -485,22 +487,25 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. - // We use an float epsilon delta to test float equality. - if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { - mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1); + // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints + // for logic state parameters + + if (static_cast(cmd->param1 + 0.5f) != 0 && static_cast(cmd->param1 + 0.5f) != 1) { + mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1); } else { + bool cmd_arms = (static_cast(cmd->param1 + 0.5f) == 1); + // Flick to inair restore first if this comes from an onboard system - if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { - status->arming_state = ARMING_STATE_IN_AIR_RESTORE; + if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { + status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE; } - transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); + transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); if (arming_res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd"); cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { @@ -512,20 +517,24 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_OVERRIDE_GOTO: { // TODO listen vehicle_command topic directly from navigator (?) - unsigned int mav_goto = cmd->param1; + + // Increase by 0.5f and rely on the integer cast + // implicit floor(). This is the *safest* way to + // convert from floats representing small ints to actual ints. + unsigned int mav_goto = (cmd->param1 + 0.5f); if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; - mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); + status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER; + mavlink_log_critical(mavlink_fd, "Pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); + status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION; + mavlink_log_critical(mavlink_fd, "Continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", + mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", (double)cmd->param1, (double)cmd->param2, (double)cmd->param3, @@ -543,7 +552,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe //XXX: to enable the parachute, a param needs to be set //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { + if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -561,7 +570,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (use_current) { /* use current position */ - if (status->condition_global_position_valid) { + if (status_local->condition_global_position_valid) { home->lat = global_pos->lat; home->lon = global_pos->lon; home->alt = global_pos->alt; @@ -598,7 +607,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } /* mark home position as set */ - status->condition_home_position_valid = true; + status_local->condition_home_position_valid = true; } } break; -- cgit v1.2.3 From 3ec9ffa66166b10101887cc4077d1b02d4a0897b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:42:02 +0200 Subject: Buzzer and led: Fix -Wshadow warnings. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 009e0ac01..85c9ccbaa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -697,11 +697,11 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds"); + warnx("ERROR: LED INIT FAIL"); } if (buzzer_init() != OK) { - warnx("ERROR: Failed to initialize buzzer"); + warnx("ERROR: BUZZER INIT FAIL"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); -- cgit v1.2.3 From 76f7960d77046c33a771be49b57e32c957e7a2ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:43:56 +0200 Subject: Mavlink text feedback: Remove now unneeded audio tag for critical messages, make overall printing more efficient. Remove buffers where no buffers are needed. --- src/modules/commander/commander.cpp | 55 +++++++++++++++++-------------------- 1 file changed, 25 insertions(+), 30 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 85c9ccbaa..76cfd5feb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -775,8 +775,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); } else { - warnx("reading mission state failed"); - mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + const char *missionfail = "reading mission state failed"; + warnx("%s", missionfail); + mavlink_log_critical(mavlink_fd, missionfail); /* initialize mission state in dataman */ mission.dataman_id = 0; @@ -789,8 +790,6 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(offboard_mission), mission_pub, &mission); } - mavlink_log_info(mavlink_fd, "[cmd] started"); - int ret; pthread_attr_t commander_low_prio_attr; @@ -1083,7 +1082,7 @@ int commander_thread_main(int argc, char *argv[]) arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } } @@ -1187,10 +1186,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "#audio: LANDED"); + mavlink_log_critical(mavlink_fd, "LANDED MODE"); } else { - mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); + mavlink_log_critical(mavlink_fd, "IN AIR MODE"); } } } @@ -1270,14 +1269,14 @@ int commander_thread_main(int argc, char *argv[]) /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); + mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); + mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { @@ -1339,12 +1338,12 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + mavlink_log_critical(mavlink_fd, "detected RC signal first time"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); + mavlink_log_critical(mavlink_fd, "RC signal regained"); status_changed = true; } } @@ -1385,7 +1384,7 @@ int commander_thread_main(int argc, char *argv[]) * the system can be armed in auto if armed via the GCS. */ if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1405,16 +1404,16 @@ int commander_thread_main(int argc, char *argv[]) if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + mavlink_log_info(mavlink_fd, "ARMED by RC"); } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + mavlink_log_info(mavlink_fd, "DISARMED by RC"); } arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + mavlink_log_critical(mavlink_fd, "arming state transition denied"); tune_negative(true); } @@ -1428,12 +1427,12 @@ int commander_thread_main(int argc, char *argv[]) } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + mavlink_log_critical(mavlink_fd, "main state transition denied"); } } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } @@ -1445,14 +1444,14 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { /* handle the case where data link was regained */ if (telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i); + mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; } have_link = true; } else { if (!telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i); + mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; } } @@ -1467,7 +1466,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST"); + mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; status_changed = true; } @@ -2010,15 +2009,13 @@ set_control_mode() } void -print_reject_mode(struct vehicle_status_s *status, const char *msg) +print_reject_mode(struct vehicle_status_s *status_local, const char *msg) { hrt_abstime t = hrt_absolute_time(); if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; - char s[80]; - sprintf(s, "#audio: REJECT %s", msg); - mavlink_log_critical(mavlink_fd, s); + mavlink_log_critical(mavlink_fd, "REJECT %s", msg); /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ @@ -2033,9 +2030,7 @@ print_reject_arm(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; - char s[80]; - sprintf(s, "#audio: %s", msg); - mavlink_log_critical(mavlink_fd, s); + mavlink_log_critical(mavlink_fd, msg); tune_negative(true); } } @@ -2048,12 +2043,12 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command); tune_negative(true); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command); tune_negative(true); break; @@ -2064,7 +2059,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command); tune_negative(true); break; -- cgit v1.2.3 From f2b30be92ae946e8e40faade6cb10edfb037fdaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:44:27 +0200 Subject: commander status leds: Fix -Wshadow warning. --- src/modules/commander/commander.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 76cfd5feb..e5ea237fc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1650,22 +1650,22 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed) +control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed) { /* driving rgbled */ if (changed) { bool set_normal_color = false; /* set mode */ - if (status->arming_state == ARMING_STATE_ARMED) { + if (status_local->arming_state == ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); - } else if (status->arming_state == ARMING_STATE_STANDBY) { + } else if (status_local->arming_state == ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; @@ -1676,12 +1676,12 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { + if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { - if (status->condition_local_position_valid) { + if (status_local->condition_local_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); } else { @@ -1714,7 +1714,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a #endif /* give system warnings on error LED, XXX maybe add memory usage warning too */ - if (status->load > 0.95f) { + if (status_local->load > 0.95f) { if (leds_counter % 2 == 0) { led_toggle(LED_AMBER); } -- cgit v1.2.3 From 66d5bc20c095a4dbd937b21c4f5fc1a67205f2d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:45:02 +0200 Subject: commander RC handling: Fix -Wshadow warnings. --- src/modules/commander/commander.cpp | 42 ++++++++++++++++++------------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e5ea237fc..0c4d48dea 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1727,16 +1727,16 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } transition_result_t -set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) +set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_OFFBOARD); + res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { - print_reject_mode(status, "OFFBOARD"); + print_reject_mode(status_local, "OFFBOARD"); } else { return res; @@ -1751,78 +1751,78 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin case SWITCH_POS_OFF: // MANUAL if (sp_man->acro_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_ACRO); + res = main_state_transition(status_local, MAIN_STATE_ACRO); } else { - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); } // TRANSITION_DENIED is not possible here break; case SWITCH_POS_MIDDLE: // ASSIST if (sp_man->posctl_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSCTL); + res = main_state_transition(status_local, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "POSCTL"); + print_reject_mode(status_local, "POSCTL"); } // fallback to ALTCTL - res = main_state_transition(status, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } if (sp_man->posctl_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTCTL"); + print_reject_mode(status_local, "ALTCTL"); } // fallback to MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case SWITCH_POS_ON: // AUTO if (sp_man->return_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_AUTO_RTL); + res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_RTL"); + print_reject_mode(status_local, "AUTO_RTL"); // fallback to LOITER if home position not set - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } } else if (sp_man->loiter_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_LOITER"); + print_reject_mode(status_local, "AUTO_LOITER"); } else { - res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_MISSION"); + print_reject_mode(status_local, "AUTO_MISSION"); // fallback to LOITER if home position not set - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1830,21 +1830,21 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } // fallback to POSCTL - res = main_state_transition(status, MAIN_STATE_POSCTL); + res = main_state_transition(status_local, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to ALTCTL - res = main_state_transition(status, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; -- cgit v1.2.3 From afc8908d38ff2eaabc2ca35363ac04697152cb6b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:45:32 +0200 Subject: commander: More docs-only changes in headers. --- src/modules/commander/commander_helper.cpp | 10 ++++++---- src/modules/commander/commander_helper.h | 9 ++++++--- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 80e6861f6..d5fe122cb 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Anton Babushkin + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,11 @@ /** * @file commander_helper.cpp * Commander helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * */ #include diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index e75f2592f..a49c9e263 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +34,9 @@ /** * @file commander_helper.h * Commander helper functions definitions + * + * @author Thomas Gubler + * @author Julian Oes */ #ifndef COMMANDER_HELPER_H_ @@ -77,6 +78,8 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern); * Use integral of current if battery capacity known (BAT_CAPACITY parameter set), * else use simple estimate based on voltage. * + * @param voltage the current battery voltage + * @param discharged the discharged capacity * @return the estimated remaining capacity in 0..1 */ float battery_remaining_estimate_voltage(float voltage, float discharged); -- cgit v1.2.3 From 7525c290f2cdfd058d54e5e6eb2d2e522d070889 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:46:36 +0200 Subject: commander: more text feedback improvements. --- src/modules/commander/state_machine_helper.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 363f5e8eb..87a60e7fd 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -87,7 +87,7 @@ static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char *state_names[ARMING_STATE_MAX] = { +static const char * const state_names[ARMING_STATE_MAX] = { "ARMING_STATE_INIT", "ARMING_STATE_STANDBY", "ARMING_STATE_ARMED", @@ -160,7 +160,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!"); valid_transition = false; } @@ -625,7 +625,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING"); failed = true; goto system_eval; } @@ -633,7 +633,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION"); failed = true; goto system_eval; } @@ -647,14 +647,14 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); - mavlink_log_info(mavlink_fd, "#audio: hold still while arming"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE"); + mavlink_log_critical(mavlink_fd, "hold still while arming"); /* this is frickin' fatal */ failed = true; goto system_eval; } } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ"); /* this is frickin' fatal */ failed = true; goto system_eval; @@ -667,7 +667,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd <= 0) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; } @@ -678,11 +678,11 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret == sizeof(diff_pres)) { if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) { - mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + mavlink_log_critical(mavlink_fd, "ARM WARNING: AIRSPEED CALIBRATION MISSING"); // XXX do not make this fatal yet } } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED READ"); /* this is frickin' fatal */ failed = true; goto system_eval; -- cgit v1.2.3 From 0d51230d4e3b87dd67819e594895b24a0a8a0306 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:04:43 +0200 Subject: commander: Final MAVLink text feedback cleanup --- src/modules/commander/state_machine_helper.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87a60e7fd..56c59ad3d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -171,16 +171,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if power is not good if (!status->condition_power_input_valid) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); valid_transition = false; } // Fail transition if power levels on the avionics rail // are measured but are insufficient - if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.9f)) { + if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.9f))) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); valid_transition = false; } } -- cgit v1.2.3 From a32577377b8d735c77ffaaee15e2bbfa74be703f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:38:43 +0200 Subject: EKF init improvements --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 8 ++++++++ src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 16 +++++++++++----- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5ce6bdb0a..ee1561280 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -610,6 +610,10 @@ FixedwingEstimator::check_filter_state() rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); + // rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); + // rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); + // rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); + // rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); @@ -1246,6 +1250,10 @@ FixedwingEstimator::task_main() _ekf->FuseVelposNED(); } else if (!_gps_initialized) { + + // force static mode + _ekf->staticMode = true; + // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index f33a1d780..e83c37bbd 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2206,7 +2206,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d void AttPosEKF::OnGroundCheck() { - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } @@ -2806,12 +2806,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) current_ekf_state.statesNaN = false; current_ekf_state.velHealth = true; - //current_ekf_state.posHealth = ?; - //current_ekf_state.hgtHealth = ?; + current_ekf_state.posHealth = true; + current_ekf_state.hgtHealth = true; current_ekf_state.velTimeout = false; - //current_ekf_state.posTimeout = ?; - //current_ekf_state.hgtTimeout = ?; + current_ekf_state.posTimeout = false; + current_ekf_state.hgtTimeout = false; + + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; // Fill variables with valid data velNED[0] = initvelNED[0]; -- cgit v1.2.3 From 744eed91dca8a5f2ad8ead7ffa812d25755bcc93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:43:10 +0200 Subject: Fix calibration counter usage --- src/modules/commander/airspeed_calibration.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index d52cafb8f..a2f11b14a 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -168,7 +168,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter = 0; /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 10) { + while (calibration_counter < 300) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -182,16 +182,19 @@ int do_airspeed_calibration(int mavlink_fd) float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; - if (fabsf(calibrated_pa) < 9.0f) { - mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%.1f Pa, #h101)", - (double)calibrated_pa); - usleep(3000 * 1000); + if (fabsf(calibrated_pa) < 50.0f) { + if (calibration_counter % 20 == 0) { + mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", + (int)calibrated_pa); + } + usleep(100 * 1000); + calibration_counter++; continue; } /* do not allow negative values */ if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static vs dynamic ports,restart"); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static vs dynamic ports,restart", (int)calibrated_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -209,8 +212,8 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); return ERROR; } else { - mavlink_log_info(mavlink_fd, "positive pressure: (%.1f Pa)", - (double)diff_pres.differential_pressure_raw_pa); + mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + (int)calibrated_pa); break; } -- cgit v1.2.3 From 8590d555b432f206e2b88893ea5198c1398aa99c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:44:57 +0200 Subject: Fix calibration counter usage, take every sample --- src/modules/commander/airspeed_calibration.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a2f11b14a..10d04aae9 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -167,8 +167,8 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter = 0; - /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 300) { + /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ + while (calibration_counter < 1500) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -181,14 +181,13 @@ int do_airspeed_calibration(int mavlink_fd) orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; + calibration_counter++; if (fabsf(calibrated_pa) < 50.0f) { - if (calibration_counter % 20 == 0) { + if (calibration_counter % 100 == 0) { mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", (int)calibrated_pa); } - usleep(100 * 1000); - calibration_counter++; continue; } -- cgit v1.2.3 From c610050dc305884a704b51c00cfff5f2f9984bcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 14:55:23 +0200 Subject: EKF hotfix: Add missing vector zero calls --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index f33a1d780..3da92b264 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2280,7 +2280,7 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max) } if (!isfinite(val)) { - ekf_debug("constrain: non-finite!"); + //ekf_debug("constrain: non-finite!"); } return ret; @@ -2920,6 +2920,8 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + dAngIMU.zero(); + dVelIMU.zero(); lastGyroOffset.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { -- cgit v1.2.3 From b4b3a2a2c68a523af5141a4452e533befc873384 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 14:58:57 +0200 Subject: EKF hotfix: Force zero initialization of vectors --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 ++-- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 2 +- src/modules/ekf_att_pos_estimator/estimator_utilities.h | 8 ++++++++ 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5d768b73d..a835599e7 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -743,8 +743,8 @@ FixedwingEstimator::task_main() /* sets also parameters in the EKF object */ parameters_update(); - Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; - Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; + Vector3f lastAngRate; + Vector3f lastAccel; /* wakeup source(s) */ struct pollfd fds[2]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 3da92b264..a41b58250 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -73,7 +73,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float qUpdated[4]; float quatMag; float deltaQuat[4]; - const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; + const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS); // Remove sensor bias errors correctedDelAng.x = dAngIMU.x - states[10]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 5648cb05c..0cafdc808 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -19,6 +19,14 @@ public: float y; float z; + Vector3f() { zero(); } + + Vector3f(float a, float b, float c) : + x(a), + y(b), + z(c) + {} + float length(void) const; void zero(void); }; -- cgit v1.2.3 From 3d505c6f42779eddc983e0f1a25c59d998cdd041 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 15:38:42 +0200 Subject: EKF hotfix: Force all fields to initialized --- .../ekf_att_pos_estimator/estimator_23states.cpp | 153 +++++++++++++++------ 1 file changed, 113 insertions(+), 40 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index a41b58250..7f9486eb5 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -5,47 +5,120 @@ #define EKF_COVARIANCE_DIVERGED 1.0e8f -AttPosEKF::AttPosEKF() - - /* NOTE: DO NOT initialize class members here. Use ZeroVariables() - * instead to allow clean in-air re-initialization. - */ +AttPosEKF::AttPosEKF() : + covTimeStepMax(0.0f), + covDelAngMax(0.0f), + rngFinderPitch(0.0f), + a1(0.0f), + a2(0.0f), + a3(0.0f), + yawVarScale(0.0f), + windVelSigma(0.0f), + dAngBiasSigma(0.0f), + dVelBiasSigma(0.0f), + magEarthSigma(0.0f), + magBodySigma(0.0f), + gndHgtSigma(0.0f), + vneSigma(0.0f), + vdSigma(0.0f), + posNeSigma(0.0f), + posDSigma(0.0f), + magMeasurementSigma(0.0f), + airspeedMeasurementSigma(0.0f), + gyroProcessNoise(0.0f), + accelProcessNoise(0.0f), + EAS2TAS(1.0f), + magstate{}, + resetMagState{}, + KH{}, + KHP{}, + P{}, + Kfusion{}, + states{}, + resetStates{}, + storedStates{}, + statetimeStamp{}, + statesAtVelTime{}, + statesAtPosTime{}, + statesAtHgtTime{}, + statesAtMagMeasTime{}, + statesAtVtasMeasTime{}, + statesAtRngTime{}, + statesAtOptFlowTime{}, + correctedDelAng(), + correctedDelVel(), + summedDelAng(), + summedDelVel(), + accNavMag(), + earthRateNED(), + angRate(), + lastGyroOffset(), + delAngTotal(), + Tbn(), + Tnb(), + accel(), + dVelIMU(), + dAngIMU(), + dtIMU(0), + fusionModeGPS(0), + innovVelPos{}, + varInnovVelPos{}, + velNED{}, + accelGPSNED{}, + posNE{}, + hgtMea(0.0f), + baroHgtOffset(0.0f), + rngMea(0.0f), + innovMag{}, + varInnovMag{}, + magData{}, + losData{}, + innovVtas(0.0f), + innovRng(0.0f), + innovOptFlow{}, + varInnovOptFlow{}, + varInnovVtas(0.0f), + varInnovRng(0.0f), + VtasMeas(0.0f), + magDeclination(0.0f), + latRef(0.0f), + lonRef(-M_PI_F), + hgtRef(0.0f), + refSet(false), + magBias(), + covSkipCount(0), + gpsLat(0.0), + gpsLon(-M_PI), + gpsHgt(0.0f), + GPSstatus(0), + baroHgt(0.0f), + statesInitialised(false), + fuseVelData(false), + fusePosData(false), + fuseHgtData(false), + fuseMagData(false), + fuseVtasData(false), + fuseRngData(false), + fuseOptFlowData(false), + + inhibitWindStates(true), + inhibitMagStates(true), + inhibitGndHgtState(true), + + onGround(true), + staticMode(true), + useAirspeed(true), + useCompass(true), + useRangeFinder(false), + useOpticalFlow(false), + + ekfDiverged(false), + lastReset(0), + current_ekf_state{}, + last_ekf_error{}, + numericalProtection(true), + storeIndex(0) { - summedDelAng.zero(); - summedDelVel.zero(); - - fusionModeGPS = 0; - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - onGround = true; - staticMode = true; - useAirspeed = true; - useCompass = true; - useRangeFinder = true; - useOpticalFlow = true; - numericalProtection = true; - refSet = false; - storeIndex = 0; - gpsHgt = 0.0f; - baroHgt = 0.0f; - GPSstatus = 0; - VtasMeas = 0.0f; - magDeclination = 0.0f; - dAngIMU.zero(); - dVelIMU.zero(); - velNED[0] = 0.0f; - velNED[1] = 0.0f; - velNED[2] = 0.0f; - accelGPSNED[0] = 0.0f; - accelGPSNED[1] = 0.0f; - accelGPSNED[2] = 0.0f; - delAngTotal.zero(); - ekfDiverged = false; - lastReset = 0; - memset(&last_ekf_error, 0, sizeof(last_ekf_error)); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); ZeroVariables(); -- cgit v1.2.3 From e8855423bea0938c607ed2ab8bde5ebec094b5a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 15:39:35 +0200 Subject: EKF hotfix: Remove unused variables, use default initializer list for vectors --- src/modules/ekf_att_pos_estimator/estimator_23states.h | 2 -- src/modules/ekf_att_pos_estimator/estimator_utilities.h | 4 +--- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index ff311649a..faa6735ca 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -172,8 +172,6 @@ public: unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction // GPS input data variables - float gpsCourse; - float gpsVelD; double gpsLat; double gpsLon; float gpsHgt; diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 0cafdc808..6d1f47b68 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -19,9 +19,7 @@ public: float y; float z; - Vector3f() { zero(); } - - Vector3f(float a, float b, float c) : + Vector3f(float a=0.0f, float b=0.0f, float c=0.0f) : x(a), y(b), z(c) -- cgit v1.2.3 From d4a867071a2183069d8902292357133856cd2ffd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 15:44:15 +0200 Subject: airspeed: Better calibration messages --- src/modules/commander/airspeed_calibration.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 10d04aae9..469f15632 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -193,7 +193,7 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "%d Pa: swap static vs dynamic ports,restart", (int)calibrated_pa); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)calibrated_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -206,9 +206,12 @@ int do_airspeed_calibration(int mavlink_fd) } /* save */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0); (void)param_save_default(); close(diff_pres_sub); + + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", -- cgit v1.2.3 From a1c5f87deaf3e8626f838b191be5b57f33d3dc46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 16:26:47 +0200 Subject: Fix C++ warnings --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 13 ++++++++++++- src/modules/ekf_att_pos_estimator/estimator_utilities.cpp | 7 +++++-- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index a835599e7..bc40ce3c4 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -112,6 +112,10 @@ public: */ FixedwingEstimator(); + /* we do not want people ever copying this class */ + FixedwingEstimator(const FixedwingEstimator& that) = delete; + FixedwingEstimator operator=(const FixedwingEstimator&) = delete; + /** * Destructor, also kills the sensors task. */ @@ -362,9 +366,10 @@ FixedwingEstimator::FixedwingEstimator() : _mag_offsets({}), #ifdef SENSOR_COMBINED_SUB - _sensor_combined({}), + _sensor_combined{}, #endif + _pos_ref{}, _baro_ref(0.0f), _baro_ref_offset(0.0f), _baro_gps_offset(0.0f), @@ -381,12 +386,18 @@ FixedwingEstimator::FixedwingEstimator() : /* states */ _baro_init(false), _gps_initialized(false), + _gps_start_time(0), + _filter_start_time(0), + _last_sensor_timestamp(0), + _last_run(0), _gyro_valid(false), _accel_valid(false), _mag_valid(false), _ekf_logging(true), _debug(0), _mavlink_fd(-1), + _parameters{}, + _parameter_handles{}, _ekf(nullptr), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 29a8c8d1e..77cc1eeeb 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -45,8 +45,11 @@ void Vector3f::zero(void) z = 0.0f; } -Mat3f::Mat3f() { - identity(); +Mat3f::Mat3f() : + x{1.0f, 0.0f, 0.0f}, + y{0.0f, 1.0f, 0.0f}, + z{0.0f, 0.0f, 1.0f} +{ } void Mat3f::identity() { -- cgit v1.2.3 From 17c5e925fb928d9c0926d8495a2db7a7b464c15f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 16:27:08 +0200 Subject: Enable C++ warnings for EKF --- src/modules/ekf_att_pos_estimator/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index dc5220bf0..36d854ddd 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -41,3 +41,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ estimator_23states.cpp \ estimator_utilities.cpp + +EXTRACXXFLAGS = -Weffc++ -- cgit v1.2.3 From f219c05f0f53ee8b8f5dbe24318678be6c255dd9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 16:27:30 +0200 Subject: Fix a set of C++ warnings in mathlib --- src/lib/mathlib/math/Matrix.hpp | 23 ++++++++++++++--------- src/lib/mathlib/math/Vector.hpp | 9 ++++++--- 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index ea0cf4ca1..c2109b479 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -69,27 +69,32 @@ public: /** * trivial ctor - * note that this ctor will not initialize elements + * Initializes the elements to zero. */ - MatrixBase() { - arm_mat = {M, N, &data[0][0]}; + MatrixBase() : + data{}, + arm_mat{M, N, &data[0][0]} + { } /** * copyt ctor */ - MatrixBase(const MatrixBase &m) { - arm_mat = {M, N, &data[0][0]}; + MatrixBase(const MatrixBase &m) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, m.data, sizeof(data)); } - MatrixBase(const float *d) { - arm_mat = {M, N, &data[0][0]}; + MatrixBase(const float *d) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, d, sizeof(data)); } - MatrixBase(const float d[M][N]) { - arm_mat = {M, N, &data[0][0]}; + MatrixBase(const float d[M][N]) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, d, sizeof(data)); } diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index c7323c215..5017b7c79 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -69,10 +69,13 @@ public: /** * trivial ctor - * note that this ctor will not initialize elements + * initializes elements to zero */ - VectorBase() { - arm_col = {N, 1, &data[0]}; + VectorBase() : + data{}, + arm_col{N, 1, &data[0]} + { + } /** -- cgit v1.2.3 From 36c20681621c2b99c1486aa0e3700cdbd86bd42d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:42:34 +0200 Subject: Removed spurious submodule entry --- src/modules/ekf_att_pos_estimator/InertialNav | 1 - 1 file changed, 1 deletion(-) delete mode 160000 src/modules/ekf_att_pos_estimator/InertialNav diff --git a/src/modules/ekf_att_pos_estimator/InertialNav b/src/modules/ekf_att_pos_estimator/InertialNav deleted file mode 160000 index 8b65d755b..000000000 --- a/src/modules/ekf_att_pos_estimator/InertialNav +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8b65d755b8c4834f90a323172c25d91c45068e21 -- cgit v1.2.3 From 1dffa750d8b91af8f600ba4cf1fbcdcc61edf80f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:44:32 +0200 Subject: added detailed print --- src/modules/commander/airspeed_calibration.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 469f15632..bc12037c3 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -162,6 +162,8 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } + mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)calibrated_pa); + /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); -- cgit v1.2.3 From 56ad0c708d2695515489802676cc295f040b3606 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:53:50 +0200 Subject: Fixed compile error --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index bc12037c3..2e3f89e65 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -162,7 +162,7 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)calibrated_pa); + mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); -- cgit v1.2.3 From 7968c6864e1255b4a65427187119aec2c3fc7ae0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 00:04:02 +0200 Subject: Force update of offset, do not add offset in final value --- src/modules/commander/airspeed_calibration.cpp | 21 +++++++++++++++------ src/modules/commander/state_machine_helper.cpp | 4 ++-- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 2e3f89e65..ea81ac6a6 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -140,6 +140,16 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { + int fd_scale = open(AIRSPEED_DEVICE_PATH, 0); + airscale.offset_pa = diff_pres_offset; + if (fd_scale > 0) { + if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + mavlink_log_critical(mavlink_fd, "airspeed offset update failed"); + } + + close(fd_scale); + } + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); @@ -182,20 +192,19 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; calibration_counter++; - if (fabsf(calibrated_pa) < 50.0f) { + if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 100 == 0) { mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", - (int)calibrated_pa); + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ - if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)calibrated_pa); + if (diff_pres.differential_pressure_raw_pa < 0.0f) { + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -217,7 +226,7 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } else { mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", - (int)calibrated_pa); + (int)diff_pres.differential_pressure_raw_pa); break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 09ea12c38..372ba9d7d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -668,8 +668,8 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) struct airspeed_s airspeed; - if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) || - hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) { + if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || + (hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) { mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; -- cgit v1.2.3 From 180b83cc6dc6872c5248993b5409ac835a201114 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 00:41:12 +0200 Subject: Better analog error handling --- src/modules/commander/airspeed_calibration.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index ea81ac6a6..92d50a025 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -94,9 +94,17 @@ int do_airspeed_calibration(int mavlink_fd) } if (!paramreset_successful) { - warn("FAILED to reset - assuming analog"); - mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + /* only warn if analog scaling is zero */ + float analog_scaling = 0.0f; + param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)) + if (fabsf(analog_scaling) < 0.1f) { + mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + close(diff_pres_sub); + return ERROR; + } + + /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); -- cgit v1.2.3 From ec8d395a2d270bd77e873f616a53ee0771c94165 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:07:30 +0200 Subject: build fix --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 92d50a025..423d88682 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -97,7 +97,7 @@ int do_airspeed_calibration(int mavlink_fd) /* only warn if analog scaling is zero */ float analog_scaling = 0.0f; - param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)) + param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); close(diff_pres_sub); -- cgit v1.2.3 From 5f176d14fed9ace9f5265a40b7ace1e6d00a7690 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:11:33 +0200 Subject: ekf: Only return from start handler with all allocations done --- .../ekf_att_pos_estimator_main.cpp | 25 ++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index a835599e7..247814a7e 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -124,6 +124,13 @@ public: */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + /** * Print the current status. */ @@ -151,7 +158,8 @@ public: private: bool _task_should_exit; /**< if true, sensor task should exit */ - int _estimator_task; /**< task handle for sensor task */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _estimator_task; /**< task handle for sensor task */ #ifndef SENSOR_COMBINED_SUB int _gyro_sub; /**< gyro sensor subscription */ int _accel_sub; /**< accel sensor subscription */ @@ -313,12 +321,13 @@ namespace estimator #endif static const int ERROR = -1; -FixedwingEstimator *g_estimator; +FixedwingEstimator *g_estimator = nullptr; } FixedwingEstimator::FixedwingEstimator() : _task_should_exit(false), + _task_running(false), _estimator_task(-1), /* subscriptions */ @@ -772,6 +781,8 @@ FixedwingEstimator::task_main() _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; + _task_running = true; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -1518,6 +1529,8 @@ FixedwingEstimator::task_main() perf_end(_loop_perf); } + _task_running = false; + warnx("exiting.\n"); _estimator_task = -1; @@ -1670,6 +1683,14 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (estimator::g_estimator == nullptr || !estimator::g_estimator->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } -- cgit v1.2.3 From 8a0e83c9cd9a54f09c6bc5c2c89917d87a451192 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:11:55 +0200 Subject: fw att ctrl: Only return from start handler with all allocations done --- src/modules/fw_att_control/fw_att_control_main.cpp | 28 ++++++++++++++++++---- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 3cd4ce928..4cdba735a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -99,13 +98,21 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle for sensor task */ int _att_sub; /**< vehicle attitude subscription */ @@ -276,6 +283,7 @@ private: * Main sensor collection task. */ void task_main(); + }; namespace att_control @@ -287,12 +295,13 @@ namespace att_control #endif static const int ERROR = -1; -FixedwingAttitudeControl *g_control; +FixedwingAttitudeControl *g_control = nullptr; } FixedwingAttitudeControl::FixedwingAttitudeControl() : _task_should_exit(false), + _task_running(false), _control_task(-1), /* subscriptions */ @@ -598,6 +607,8 @@ FixedwingAttitudeControl::task_main() fds[1].fd = _att_sub; fds[1].events = POLLIN; + _task_running = true; + while (!_task_should_exit) { static int loop_counter = 0; @@ -921,6 +932,7 @@ FixedwingAttitudeControl::task_main() warnx("exiting.\n"); _control_task = -1; + _task_running = false; _exit(0); } @@ -966,6 +978,14 @@ int fw_att_control_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } -- cgit v1.2.3 From efa5d8f57f303a51f618b7c108f516d5e857c3b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:12:17 +0200 Subject: fw pos ctrl: Only return from start handler with all allocations done --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 26 +++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 98ccd09a5..08c996ebc 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -120,10 +119,18 @@ public: */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + private: int _mavlink_fd; bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle for sensor task */ int _global_pos_sub; @@ -391,13 +398,14 @@ namespace l1_control #endif static const int ERROR = -1; -FixedwingPositionControl *g_control; +FixedwingPositionControl *g_control = nullptr; } FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), _task_should_exit(false), + _task_running(false), _control_task(-1), /* subscriptions */ @@ -1290,6 +1298,8 @@ FixedwingPositionControl::task_main() fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; + _task_running = true; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -1390,6 +1400,8 @@ FixedwingPositionControl::task_main() perf_end(_loop_perf); } + _task_running = false; + warnx("exiting.\n"); _control_task = -1; @@ -1478,6 +1490,14 @@ int fw_pos_control_l1_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (l1_control::g_control == nullptr || !l1_control::g_control->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } -- cgit v1.2.3 From 9ce7820e419d2ffa379fb7a3cc168f500623fa3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:32:51 +0200 Subject: Make instructions in commander more obvious for airspeed calibration --- src/modules/commander/airspeed_calibration.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 423d88682..598cfe9e2 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,15 +180,16 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); calibration_counter = 0; + const int maxcount = 3000; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ - while (calibration_counter < 1500) { + while (calibration_counter < maxcount) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -204,7 +205,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 100 == 0) { - mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", + mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -212,6 +213,8 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { + mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); @@ -246,6 +249,12 @@ int do_airspeed_calibration(int mavlink_fd) } } + if (calibration_counter == maxcount) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); -- cgit v1.2.3 From f3549d775cb049bcde93c3e860c3adbad3763364 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:33:35 +0200 Subject: Airspeed driver: Use the known sensor offset for raw value as well --- src/drivers/ets_airspeed/ets_airspeed.cpp | 3 +++ src/drivers/meas_airspeed/meas_airspeed.cpp | 5 ++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index d1a9fa57c..c15a0cee4 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -172,6 +172,9 @@ ETSAirspeed::collect() diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset; } + // The raw value still should be compensated for the known offset + diff_pres_pa_raw -= _diff_pres_offset; + // Track maximum differential pressure measured (so we can work out top speed). if (diff_pres_pa > _max_differential_pressure_pa) { _max_differential_pressure_pa = diff_pres_pa; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 7763f1057..07611f903 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -225,7 +225,10 @@ MEASAirspeed::collect() // correct for 5V rail voltage if possible voltage_correction(diff_press_pa_raw, temperature); - float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset); + // the raw value still should be compensated for the known offset + diff_press_pa_raw -= _diff_pres_offset; + + float diff_press_pa = fabsf(diff_press_pa_raw); /* note that we return both the absolute value with offset -- cgit v1.2.3 From 83652794c7709ee8058587a99e81826d4959ed49 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 15 Jul 2014 00:06:24 +0200 Subject: Add a mixer for the Phantom so it can be uniquely adjusted for this airframe. --- ROMFS/px4fmu_common/init.d/3031_phantom | 2 +- ROMFS/px4fmu_common/mixers/phantom.mix | 69 +++++++++++++++++++++++++++++++++ 2 files changed, 70 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/mixers/phantom.mix diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index d05c3174f..654b0bdab 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -36,7 +36,7 @@ then param set FW_T_TIME_CONST 5 fi -set MIXER FMU_Q +set MIXER phantom # Provide ESC a constant 1000 us pulse set PWM_OUTPUTS 4 diff --git a/ROMFS/px4fmu_common/mixers/phantom.mix b/ROMFS/px4fmu_common/mixers/phantom.mix new file mode 100644 index 000000000..7fe6dc055 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/phantom.mix @@ -0,0 +1,69 @@ +Delta-wing mixer for PX4FMU +=========================== + +Designed for the Phantom FX-61 + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4/Pixhawk. The configuration assumes the elevon servos are connected to +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor are set so that pitch will have more travel than roll. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 6500 6500 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 -6500 -6500 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 -- cgit v1.2.3 From 9f837267de861cfd8313984f2ec64b8482a72161 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 15 Jul 2014 00:10:08 +0200 Subject: Updated the comments. --- ROMFS/px4fmu_common/mixers/phantom.mix | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/phantom.mix b/ROMFS/px4fmu_common/mixers/phantom.mix index 7fe6dc055..00c37a16c 100644 --- a/ROMFS/px4fmu_common/mixers/phantom.mix +++ b/ROMFS/px4fmu_common/mixers/phantom.mix @@ -1,7 +1,5 @@ -Delta-wing mixer for PX4FMU -=========================== - -Designed for the Phantom FX-61 +Phantom FX-61 mixer +=================== This file defines mixers suitable for controlling a delta wing aircraft using PX4/Pixhawk. The configuration assumes the elevon servos are connected to -- cgit v1.2.3 From 61286451361c0e4547caf2385f56ee6cc8afffb1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 07:37:17 +0200 Subject: mathlib: More C++ fixes --- src/lib/mathlib/math/Matrix.hpp | 2 ++ src/lib/mathlib/math/Vector.hpp | 12 ++++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index c2109b479..ca931e2da 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -77,6 +77,8 @@ public: { } + virtual ~MatrixBase() {}; + /** * copyt ctor */ diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 5017b7c79..0ddf77615 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -78,19 +78,23 @@ public: } + virtual ~VectorBase() {}; + /** * copy ctor */ - VectorBase(const VectorBase &v) { - arm_col = {N, 1, &data[0]}; + VectorBase(const VectorBase &v) : + arm_col{N, 1, &data[0]} + { memcpy(data, v.data, sizeof(data)); } /** * setting ctor */ - VectorBase(const float d[N]) { - arm_col = {N, 1, &data[0]}; + VectorBase(const float d[N]) : + arm_col{N, 1, &data[0]} + { memcpy(data, d, sizeof(data)); } -- cgit v1.2.3 From e91a4217ad23a0f5190b0e34b7621a5170632a43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 07:54:12 +0200 Subject: Fix warning --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 598cfe9e2..0e58c68b6 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -186,7 +186,7 @@ int do_airspeed_calibration(int mavlink_fd) usleep(500 * 1000); calibration_counter = 0; - const int maxcount = 3000; + const unsigned maxcount = 3000; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { -- cgit v1.2.3